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ABSTRACT 


Robot manipulators have been studied, using various approaches 
to obtain the kinematic and dynamic equations which describe their 
motion. Conventional body-oriented robot arm kinematic equations 
have the disadvantage that a singular condition occurs when two suc- 
cessive links of the manipulator are aligned. When this occurs, the 
jacobian matrix which relates the end effector motion to the joint 
angle variations becomes singular and is not invertible, resulting in 
motion that can not be simulated. This thesis extends the previous 
work done in the investigation of a nonsingular Newton Euler 
approach to forward dynamic equations interpreted in a global 
(inertia) fixed reference frame. Specifically, the previous results are 
extended into validation of the approach for three-dimensional 
motion, including gravitational effects. In addition, the comparison 
and verification of the motion of an actual robotic manipulator to the 


Simulation is investigated. 
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I. INTRODUCTION 


The study of robotics is a fairly new area in the fields of science 
and engineering which has fundamentals that can be traced back to 
many basic disciplines, such as electrical engineering, mechanical 
engineering, and computer science. The mechanical engineering 
interest in the study of robotic manipulators can be traced back to as 
early as the 1940s, although the first industrial robots were not intro- 
duced until the late 1950s and early 1960s [Ref. 1]. Mechanical engi- 
neers were mostly interested in two areas of robotics: the control of 
the linkage and the study of its motion. While these two areas are 
interrelated, this thesis focuses on the second: the study of the motion 
of the linkage. 

The study of robot motion (dynamics) is further divided into the 
study of arm dynamics and robot arm kinematics. Both these divisions 
are subdivided into a direct and an inverse problem. The direct 
dynamics problem is the calculation of link state variables such as 
acceleration, velocity, and joint angles from the known joint torques. 
The inverse dynamic problem assumes state variables are known and 
the joint torques are to be calculated. In the kinematics, the direct 
problem is the determination of the end effector position and its 
orientation from a given set of link geometries and joint angles. The 
inverse problem is the calculation of the link geometries and joint 


angles for a given end effector position. 


Various methods to solve the inverse kinematic problem use 
matrix transformations, inversion, and multiplication, and as such are 
subjected to singularity problems [Ref. 2]. Singularity occurs when two 
successive links are aligned (i.e., the angle between the links is O or 
180 degrees). When a singularity occurs, the matrices involved are also 
singular and not invertible. 

A method to avoid the singularity problem was developed at the 
Naval Postgraduate School using Newtonian dynamics in terms of a 
globally fixed coordinate system [Ref. 3]. This method treats each link 
as a free body with the forces and moments applied at the joints and 
the use of Newton’s law to derive the equations of motion. In this 
thesis, the development of the free body analysis approach to rigid 
manipulators dynamics is contined and extended to three dimen- 


sions and including gravitational effects. 


II. PROBLEM STATEMENT 


Theoretical dynamic approaches are well developed and have 
been extensively used with simulations. However, in manipulator 
studies, these accepted methods all have a singularity that arises when 
two successive links are aligned. This does not allow the motion to be 
simulated [Ref. 3]. This shortfall cannot be tolerated in the modelling 
of an actual robot or robotic manipulator because it is of the utmost 
importance to accurately know the arm position at all times (for real- 
time control purposes) [Ref. 2]. Therefore, the problem directed by 
this thesis research project was: 


Continue the development of the Newton Euler free-body approach 
for a three-link manipulator extended to three-dimensional motion 
and including gravitational effects. In addition, compare simulation 
results with those of an actual robotic arm. [Ref. 4] 


III. BACKGROUND 


A. THEORY 

The basic theory for the Newton Euler free-body analysis to over- 
come the singularity problems of conventional robot dynamics was 
developed by Sadrettin Altinok [Ref. 3]. The theory of treating each 
link of the manipulator as a free body with respect to a global coordi- 


nate system leads to the following 27 equations for a three-link 





manipulator. 

FXO + M1*AX1] - FX] =O (1) 
FYO + M1*AY1 - FY1 =0 (2) 
FZO + M1*AZ1 - FZ1 = -WGl (3) 
AX1 + RB1Z*WD1Y - RB1X*WD1Z = AXO - MICO (4) 

where MICO = (X component of) 
WD1 X RBG1 + W1 X (W1 X RBG) (4a) 
AY1 - RB1IZ*WD1X + RB1IX*WD1Z = AYO - MJCO (5) 


where MJCO = (Y component of equation 4a). 


AZ1 + RBY1*WD1X - RBIZ*WD1Y = AZO - MKCO (6) 
where MKCO = (Z component of equation 4a). 
RB1Z*FYO - RBLY*FZO - IXXT1*WD1X + LXYTI*WDI1Y + 


IXZT1*WD1Z - RAIZ*FY1 + RALY*FZ1] = T1X - TOX (7) 


-RB1Z*FXO + RBIX*FZO + IXYT1I*WD1X - IYYTI*WD1Y + 


IYZTI*WD1Z + RAIZ*FX1 - RALX*FZ1 = TLY - TOY (8) 


RB1LY*FXO - RBIZ*FYO + IXZT1*WD1X + TYZT1I*WD1Y - 


IZZTI*WD1Z —- RALY*FX1 + RALX*FY1 = T1Z - TOZ (9) 


FX] + M2*AX2 ~ FX2 =0 exe) 
FY] + M2*AY2 - FY2 = 0 (11; 
FZ] + M2*AZ2 - FZ2 = -WG2 (12) 
-AX1 - RAIZ*WD1Y + RALY*WD1Z + AX2 + RB2Z*WD2Y - 
RB2Y*WD2Z = MIC1 - MIC2 OL8s), 
where MIC1 = (K component of) 
D1 X RA] + W1 X (W1 X RA) (13a) 


where MIC2 + (X component of) 


D2 X RB2 + W2 X (W2 X RB2) (13b) 


-AY1]1 + RAIZ*WD1X —- RALX*WDI1Z + AY2 —- RB2Z2Z*WD2X + 


RB2X*WD2Z = MJC1 - MJC2 (14) 


where MJC1 = (Y component of equation 13a) and where MJC2 = (Y 


component of equation 13b). 


-AZ1 —- RALY*WD1X + RAIX*WDLY + AZ2 + RB2Z2Y*WD2X - 


RB2X*WD2Y = MKC] —- MKC2 (15) 


where MKC1 = (Z component of equation 13a) and where MKC2 = (Z 


component of equation 13b). 
RB2Z*FY1 - RB2Y*FZ1 - IXXT2*WD2X + [IXYT2*WD2Y + 


IXZT2*WD2Z —- RA2ZZ*FY2 + RA2Y*FZ2 = T2X - T1X (16) 


-RB2Z*FX1 + RB2X*FZ1 + IXYT2*WD2X% —- TYYT2*WD2Y + 


IYZT2*WD2Z + RA2Z*FX2 —- RAZX*FZ2 = T2Y - TLY ( 17%) 


RB2Y*FX1 + RB2X*FY1 + IXZT2*WD2X + TYZT2*WD2Y - 


IZZT2*WD2Z - RA2ZY*FX2 + RAZX*FY2 = T2Z - T1Z (18) 


FX2 + M3*AX3 = 0 (19) 
FY2 + M3*AY3 = 0 (20) 
FZ2 + M3*AZ3 = -WG3 (21) 


-AX2 - RA2Z*WD2Y + RA2ZY*WD2Z + AX3 + RB3Z*WD3BY - 


RB3Y*WD3Z = MIC3 - MIC4 (22) 


where MIC3 = (X component of) 


WD2 X RA2 + W2 X (W2 X RAZ) (22a) 


where MIC4 = (X component of) 


D3 X RB3 + W3 X (W3 X RB3) (22b) 


~AY2 + RA2ZZ*WD2X —- RA2ZX*WD2Z + AY3 —- RB3Z*WD3X + 


RB3X*WD3Z = MJC3 - MJC4 (23) 


where MJC3 = (Y component of equation 22a) and where MJC4 = (Y 


component of equation 22b). 


-AZ2 —- RA2ZY*WD2X + RA2ZX*WD2Z + AZ3 + RB3SY*WD3X - 


RB3X*WD3Y = MKC3 ~- MKC4 (24) 


where MKC3 = (Z component of equation 22a) and where MKC4 = (Z 


component of equation 22b). 


RB3Z*FY2 —- RBSY*FZ2 - IXXTS*WD3X + IXYTS*WD38Y + 


IXZT3*WD3Z = - T2X (255 


-RB3Z*FX2 + RBSX*FZ2 + IXYTS*WD3X —- IYYTS*WD3Y + 


IYZT3*WD3Z = - T2Y (26) 


RB3SY*FX2Z —- RBSX*FY2 + IXZT3S*WD3X + TYZT3S*WDsy - 


IZZT3*WD3Z = - T2Z (27) 


B. COMPUTATION 

The Dynamic Simulation Language (DSL) was used to simulate the 
direct dynamics problem. A matrix (MATA, a 27 x 27 matrix) was cre- 
ated from the known coefficients of the unknown variables in the pre- 
vious 27 equations. This was possible due to the assumption that the 
change in the inertia of the links, the link velocities, and the joint 
position must be extremely small during a simulated step interval and 
can thus be treated as being constant during the step. In the direct 
dynamics problem, a vector (MatB, 27 x 1) was generated from the 
following components: joint torques, the centers of gravity of the link, 
and the calculated cross-products of angular velocities (Figure 1). This 


gave a syStem of equations in the form of equation 28: 


ie ty 








E/E - 
Yr 
“1~=— E/E - a! 
= 
¥ | é a 
os 
an 


11 cl bt 


| LI QI 12 22 €2 #2 Se 92 ke 


i es wate: fit reas for Sifiulation 


§ 4 7 6 9 10—s tl 2 i3 #4 IS 16 17 16 19 #+$%]20 gk 22 23 24 #23 £28 = 27 


* 


2 


b 


So 


A*x=b (28) 


where A and b are known and x is an unknown vector (which contains 
the forces at the link ends and the angular and linear accelerations of 
each link). The IMSL routine LEQT2F (a linear equation solver) was 
used to obtained the vector x. [Ref. 3 and Ref. 5] 

In the inverse problem, MATA was developed the same as in the 
direct problem as discussed above, but now vector x was known (from 
the history of position data) and vector b was found by straight matrix 
multiplication of equation 28. [Ref. 3 and Ref. 5] 

The previous work used transformation matrix operations to 
obtain joint orientations [Ref. 3]. This was done because joint orienta- 
tion cannot be obtained directly from the integration of rotational 
velocity [Ref. 6 and Ref. 7]. The approach used in this thesis was one of 
simple geometric relationships. It was reasoned that the linear accel- 
erations were either known or calculated from the matrix operation of 
LEQT2F (equation 28). These linear accelerations were the linear 
accelerations of the link center of gravities with respect to the earth 
fixed (inertial) coordinate system which were integrated to obtain the 
linear velocities. The velocities were further integrated to obtain the 
position of the center of gravities. 

Knowing the earth fixed coordinates of the centers of gravity, and 
that each link can be represented by a rigid body [Ref. 8], it was 
known that the center of gravity must be on the vector from the 


inboard joint to the end of the link, or next joint (Figure 2). Once the 
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Figure 2. Link Geometries 
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vector was known to the center of gravity, the directional cosines 


were calculated from the following three equations for each link. 


DRCOSX = [LCOGX -JX(inboard)]/LNCOG (29) 
DRCOSY = [LCOGY - JY(inboard)] /LNCOG (30) 
DRCOSZ = [LCOGZ - JZ(inboard)]/LNCOG (31) 


where LCOGX is the X location of the CG with respect to the inertia 
frame; JX is the X location of the inboard joint location with respect to 
the inertia; and LNCOG is magnitude of the length from the inboard 
joint to the CG. 

Once the directional cosines were known, the same method as 
used by previous investigators was used to calculate the MATA and 


MATB matrices. 


C. CONSTRAINTS 

With the desire to simulate the motion of an actual robotic manip- 
ulator and to compare the simulation with data from the manipulator, 
constraints had to be added into the simulation so the simulation 
would take into account the the joint mechanical restrictions. In the- 
ory, each joint is a one degree of freedom connection. However, in the 
chosen arm, the zero joint rotates in the Z direction only, while joints 
one and two rotate about both the X and Y axes (Figure 3). The previ- 


ously mentioned constraints led to the following equations: 
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Figure 3. Robot Axis Rotation with Constraints 


13 


AX1 =O (32) 


AY1 =0 (33) 

AZ1 =0 (3.4) 

WDX1 = 0 (35) 
WDY1 = 0 (36) 
WDZ1 = WDZ2 = WDZ3 (3 


The concept for entering the constraints into the simulation was 
developed in Reference 3 and led to the constraint section of the DSL 
code as seen in Appendix A and Appendix B. The basic concept which 
was developed was to zero out the row only in MATA, then set the 
diagonal of that row to one and the corresponding row of MATB to 
zero. This concept was changed to include zeroing out the row and 
column of MATA corresponding to the row of vector x, which was to 
be zero, along with setting the diagonal of the row to 1 and the corre- 


sponding row of MATB to zero. 


D. GRAVITATIONAL TORQUES 
The simulation of gravitational effects on the arm required that an 


equalizing torque be applied at the joints to overcome the gravitational 
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D. GRAVITATIONAL TORQUES 

The simulation of gravitational effects on the arm required that an 
equalizing torque be applied at the joints to overcome the gravitational 
pull (Figure 4). This torque varied as the Z location of the center of 
gravity of links 2 and 3 varied because the magnitude of the moment 
arm varied as the link moved. The lever arm which affected the gravi- 
tational torque was the projection of the vector from the inboard joint 
to the link center of gravity onto the XY plane (Figure 5). Knowing this 
lever arm and the weight of the links, the following equations were 


used to calculate the equilibrium torque needed to overcome gravity: 


on = SQRT (LCOGX2*LCOGX2 + LCOGY2*LCOGY2) = (38) 
MARM2B = SQRT (LCOGX3*LCOGX3 + LCOGY3*LCOGY3) (39) 
MARMS3 = SQRT ((LCOGX3-JX2)**2 + (LCOGY3-JY2)**2) (40) 

TG] = MAR2A * WG2 + MARM2B * WG3 (41) 


TG2 = MARMS3 * WG3 (42) 


The equations above represent the magnitude of the required 


torque which must be resolved into its X and Y components (Figure 6). 
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Figure 5. Gravitational Torque Moment Arms 
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Figure 6. Gravitational Torque X and Y Components 
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With the given constraints, this was done in the following manner. 
Angle THZ was calculated from the second integral of WDZ1 (the base 
link only revolved around its vertical axis). Then its cosine and sine 
were used to obtain the X and Y components of the gravitational equi- 


librium torque as follows: 


TGX = TG * COS (THZ) (43) 


TGY = TG * SIN (THZ) (44) 


E. INERTIAS | 

The inertias of the links of the robot were calculated the first 
time from the equations of Reference 2 in the global (inertial) 
reference frame and then transferred to the local coordinates (Figure 


7) by the use of a transformation [Ref. 9], giving the following equation: 


Inertia(local) = TMAT * Inertia(global) * TMATTR (45) 


where MATA is a transformation matrix based on the link angles 
[Ref. 1 and Ref. 10]. Knowing that the local inertias remain constant, 
the global inertias for each link were calculated for each time iteration 


by the use of the inverse transformation below: 


Inertia(global) = TMATTR * Inertia(local) * TMAT (46) 
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Figure 7. Inertia Coordinate Systems 
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IV. SIMULATION NONSINGULAR MOTION VALIDATION 


A. TWO-DIMENSIONAL MOTION VALIDATION 

A known motion which had adjacent links aligned was that of the 
two-dimensional double pendulum (Figure 8). A comparison of the 
simulation to the theoretical solution was used to validate the 
nonsingularity, the motion tracking, and the gravitational effects of the 
simulation. 

The theoretical solution [Ref. 11] used a Lagrange equation which 
only included the linear kinetic energy of the system (equation 47), 
while the simulation included both the linear and rotational kinetic 
energies (equation 48). To account for the difference, the inertia in 
equation 48 was set to a small value. Note that it was not possible to 
set the inertias to zero because the inertia entries in matrix MATA 
were on the diagonal and would thus not have allowed the inverse to 


be calculated. 


Kinetic Energy = 1/2 * m * v2 (47) 


Kinetic Energy = 1/2 *m * v2 + 1/21 * w2 (48) 


The point mass method developed in Reference 3 allowed the 


first mass of the double pendulum to be set to a value equal to that of 


zal 






Figure 8. Double Pendulum Configuration 


Zee 


the outer point mass of link 2 plus the inner point mass of link 3 
(equation 49). The second mass of the double pendulum was set to the 


value of the outer point mass of link 3 (equation 50). 


Wmetep) = me(2.2)0-- m (3,1) (49) 


m2 (dp) = m (3.2) (50) 


The lengths of the double pendulum were the same as the link 
lengths (Figure 8). 

The results of the theoretical and simulation program for the 
double pendulum problem agreed (Figures 9 and 10), and would have 
been identical had the inertia been set to zero. Tracking of the motion 
through the potential singular points presented no difficulties to the 


simulation (Figure 11). 


B. THREE-DIMENSIONAL NONSINGULAR MOTION 

A motion which had potential singular points was chosen which 
validated the nonsingularity of the simulation in three dimensions 
(Figure 12). The motion which was chosen was to input the joint 1 and 
joint 2 angles as a time function and allow the third joint angle to 
remain zero, so links 2 and 3 were always aligned and potentially 
singular. This chosen motion also had all three links aligned at time 


equal to 0.785 seconds (Figure 12). 
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Figure 9. Double Pendulum Angle 2 
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Figure 10. Double Pendulum Angle 3 
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Figure 11. Double Pendulum Simulation Through 
Potential Singular Points (angle = 0) 
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Figure 12. Three-Dimensional Nonsingular Validation 
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The simulation again had no difficulties tracking the input motion 
through the potentially singular points (Figure 13). The error in the 
predicted and simulated end effector position (Figure 14) was never 


greater than 1.0 percent. 
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Figure 13. Three-Dimensional Potential Singular Points 
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Figure 14. End Effector Position Error 
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A. 


V. EXPERIMENTAL VALIDATION 


MANIPULATOR 


A robot manipulator was chosen to check the agreement of the 


simulation to data from an actual robot arm. The arm that was available 


was the NEPTUNE II (Figure 15), which is a low-performance, 


hydraulic-operated arm. This arm required certain hardware modifi- 


cations, namely: 


the remounting of the electronic solenoid boards to facilitate 
access and provide protection from hydraulic leaks; 


installation of bleed holes in the major joint pistons to allow air to 
be bled off when the system has been opened for repairs; 


design and manufacture of a pump and accumulator stand that 
would provide spray protection for the manipulator in the case of 
malfunctions; 

modification to the plumbing of the pump and accumulator to 
include an electrically controlled solenoid valve operated from the 
Electronic Control Panel (ECP); 

rewiring the pump to be electrically controlled from the ECP; 


redesign and manufacture of the waist joint (joint 0) shaft and 
coupling, along with that of several piston end plates; and 


an overhaul of the complete electronics. 


PRESSURE TRANSDUCERS 


Pressure transducers which had previously been installed on the 


NEPTUNE II arm at joints 1 and 2 (Figure 16) allowed for data collec- 


tion to validate the two-dimensional simulation with gravitational 


effects. Initially, the pressure transducers had to be calibrated. 
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Figure 15. NEPTUNE II Robot 
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Figure 16. Robot Pressure Transducers 
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A 3,000-pound nitrogen bottle with a 200 psi regulator was used 
for pressure transducer calibration. This was connected to a test rig 
which consisted of a varlinear pressure adjustment and a 200-pound 
pressure gage which had been calibrated and was incremented in 0.2- 
pound increments (Figure 17). The varlinear allowed the pressure at 
the transducer to be varied in fine increments from zero to 120 psi, 
which was the stated range of the transducers. The electrical connec- 
tions from the transducers had been hard-wired to the ECP and then 
to a digital multimeter (Fluke 8024 B); the meter 20-volt scale was 
used to read the voltage output of the transducers (the voltage was 
read to the second decimal place). Each Omega Series PX302 pressure 
transducer had two complete sets of data from O to 120 psi; the aver- 
ages of these are provided in Table 1. The pressure transducers were 
very linear from O to 115 psi, with each 5 psi producing a voltage of 
0.5 volts (Figure 18). This range of pressures was well acceptable for 


the NEPTUNE II, which operates between O and 118 psi [Ref. 12]. 
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Figure 17. Pressure Transducer Calibration Set-Up 
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Pressure 


(psi) 


OOO 
O05 
O10 
O15 
020 
O25 
030 
035 
040 
045 
O50 
O55 
O60 
O65 
O70 
O75 
O80 
O85 
O90 
O95 
100 
105 
110 
11S 
120 


TABLE 1 


PRESSURE TRANSDUCER DATA 


Pl 
volts 


-0.010 
0.500 
O7295 
1.500 
1.985 
2.500 
3.000 
3.490 
3.990 
4.500 
4.995 
9.490 
9.995 
6.490 
6.990 
7.490 
(ee Sh, 
8.490 
8.990 
9.480 
8) SSN, 

10.490 

10.990 

11.485 

11.580 


P2 
volts 


0.000 
0.510 
1.005 
1.500 
Hohe, 3 
2.500 
3.000 
3.500 
3.999 
4.500 
4.995 
5.500 
9.995 
6.500 
7.000 
7.500 
8.005 
8.510 
9.005 
9.500 
10.005 
10.510 
11.010 
11.490 
11.490 
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P3 
volts 


0.015 
0.520 
1,025 
1.530 
2.020 
2.530 
3.030 
3.530 
4.030 
4.530 
9.025 
5.920 
6.020 
6.520 
7.020 
7.510 
8.010 
8.510 
9.000 
9.500 
10.000 
10.500 
11.000 
11.490 
11.990 


P4 
volts 


0.000 
0.500 
1.005 
1.500 
2.000 
2.900 
3.000 
3.490 
3.995 
4.500 
4.990 
5.490 
9.990 
6.490 
6.990 
7.500 
8.000 
8.500 
9.000 
9.500 
10.000 
10.500 
11.000 
11.500 
12.000 


VOLTS 


V2 
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Figure 18. Pressure Transducer Data 
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C. MEASUREMENTS 

1. Link Lengths 
With the necessity to disassemble the NEPTUNE II for 
repairs, the link length measurements were physically measured. 


These measurements are provided in Table 2 to the nearest 1/8 inch. 


TABLE 2 
LINK LENGTHS 


Parameter Length Length 
inch meter 

LINKL1 POEZ 50 0.2604 
LINKL2 16.375 0.4159 
LINKL3 13.750 0.3498 


2. Link Masses 

Also during the disassembly of the robot, the link masses 
were measured using a scale which was in half-pound increments. The 
design of the robot allowed for a good approximation of the two 
masses per link as used in the simulation (Figures 19 and 20). This 
was a crude measurement, but it was felt that this was sufficient to be 
used for the simulation and that the time necessary to completely dis- 
assemble the robot in order to be able to obtain closer measurements 


was not justified. The masses which were measured are provided in 


Table 3. 
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Figure 19. Robot Link Masses 
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Figure 20. Simulation Link Masses 
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TABLE 3 
LINK MASSES 


Parameter Pounds Kilograms 
MASS(1,1) est. 5 2.273 
MASS(1,2) est. Is) 6.818 
M1 (measured) 20 9.091 
MASS(2,1) est. 1 0.455 
MASS(2,2) est. 1 0.455 
M2 (measured) 2 0.910 
MASS(3,1) est. ES: 5.909 
MASS(3,2) est. 13 5.909 
M3 (measured) 26 11.818 


3. Length from COG of Link to Center of Mass 
The point mass centers were assumed to be at the joints 
(Figure 20). A simple calculation of the moments around the center of 


gravity was performed; this provided the data in Table 4. 


TABLE 4 
LENGTH FROM LINK CENTER OF GRAVITY 
TO MASS CENTER OF GRAVITY 


Parameter Length Length 
inch meter 

L(1,1) 7.6875 0.1953 
L(1,2) 2.9625 0.0651 
Cz, 7) 8.1875 0.2080 
(22) 8.1875 0.2080 
L(3, 1) 6.8750 0.1746 
L(3,2) 6.8750 0.1746 
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D. DATA COLLECTION 

Position data and pressure data were taken for several movements 
of links 2 and 3 of the robot from the electronic control panel and 
recorded on a four-channel strip chart recorder. An example of the 


recorded data is provided (Figure 21). 


E. DATA REDUCTION 
The data taken for the various runs was reduced using the follow- 


ing equation to obtain joint torques. 


TORQUE = A PRESSURE * AREA(piston) * RADIUS(lever arm) (51) 


where AREA(piston) = 0.00312 square meters and RADJUS(lever arm) 
= 0.059772 meters: 


F. VALIDATION 

The delta pressure data versus time was entered into the robot 
validation program as a time function and the torque was calculated as 
a function of time from this data. The simulation position results 
agreed with the actual robot position in the general trend of motion 
(Figure 22). It was felt that the inaccuracies in the results were due to 
both the simulation round-off errors and the crude methods used in 
the collection of the robot link masses and link lengths, particularly 
the lengths from the center of gravity of the link to the center of 
gravity of the point masses. The scalloped shape of the simulated 


result was attributed to the normalization of the directional cosines. 
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Figure 22. Robot/Simulation Motion 
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VI. RESULTS AND RECOMMENDATIONS 
. A nonsingular dynamic simulation of a rigid revolute three-link 
manipulator has been further developed. It includes: 
e Gravity, 
e Three-Dimensional Motion, 
¢ Comparatively straightforward dynamics. 


. Accuracy and nonsingularity has been validated with a double 
pendulum. 


. Three-dimensional nonsingularity has been validated. 
. Procedures for actual robot data collection have been developed. 
. The following recommendations are provided: 


e Simulate actual three-dimensional robot performance and 
compare predicted to measured variables. 


e Enhance the computer program code to enable it to be more 
interactive. That is, let the user specify the constraints during 
execution. 


e Use this simulation to develop advanced controllers for rigid 
robot linkages. 
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APPENDIX A 


DIRECT DYNAMIC SIMULATION PROGRAM 


TITLE DIRECT DYNAMICS PROBLEM 


Fedesesesetedevedede steve tededeveveteve secede deveve sede dete tedeve Fede feds Fesedesdesedede tevesedcetedede kedetesetetedstetcsetetedtedekdestessck tev 


6s 


* 
* 
rh 


* THIS IS A PROGRAM THAT WILL SOLVE THE DIRECT DYNAMICS PROBLEM FOR A 
%* 3 LINK RIGID BODY MANIPULATOR WITHOUT THE USE OF ANY TRANSFORMATION 
* MATRICES. THE ORIGINAL PROGRAM WAS WRITTEN BY LT ATINOK AND HAS 

* HAS BEEN GREATLY MODIFIED TO INCLUDE 3 D MOTION AND GRAVITATIONAL 

* EFFECTS BY LT R. M. VERBOS USN SEPTEMBER 1988 
Tedesedevevededededededededovededs vevevede Fede Fev tess cedede sede Jods Seve sed Fess Je ede se Fe Seca cK Ge eae eee ea Tee oc ae ee ve 20 
TERMINAL 

METHODeKKSE A 

PRINT .10,DRCANX(1),DRCANY(1),DRCANZ(1),... 

DRCANX(2) , DRCANY(2) ,DRCANZ(2) , DRCANX(3) , DRCANY(3) , DRCANZ(3),.. 
DRCSX(1),DRCSY(1),DRCSZ( 1) ,DRCSX(2) ,DRCSY(2) ,DRCSZ(2) ,DRCSX(3),... 
DRGSY ( See DRG 76s ee 

JX@, IYO, JZ0-I%1, JY], 2 enn ee 

LCOGA( 1), LCOGY( 1) , LGCOGZU1) LCGGrie2 ) NeCGr (2 be Oe 

LG@GX( 3), LGOGY( 3) meoG7Ccnnraan 

TOM, TOY .TOZ. TIX, 11.717 ai ae 

WDX(1) ,WDY(1),WDZ( 1) ,WDX( 2) ,WDY(2) ,WDZ(2) , WDX(3) ,WDY(3) ,WDZ(3),... 
ANTE AY] AZT AY? AY he eAX Ay S (A7geeeee 

WIX,WIY WIZ WZKW2Y W22 WSS Woe 

FXO, FYO,FZ0.6X1. FY]. b 7a ye 

IXXT( 1), IYYT(1) , IZZTC1), IX87(2) , TYYI( 2), 1220@2 emacs a Gene 
IZZT(3) ,IXYT(C1), TYZTC1) , 1XZTC 1), IXYT(2) ,1YZ0 2) ee eee 
TYZT(3)), Zanes ee 

LNCOG1 , LNCOG2 , LNCOG3 , THZANG ,COSTHZ ,SINTHZ, UIP xS Te eZee 
ANG12X,ANG12Y,ANG12Z, ANG23X, ANG23Y,ANG23Z,... 

JEG. 2 ei bale 

CONTROL FINTIM =1.5, DELT = .0005, DELMAX = .1, DELPRT = .10 

D DIMENSION MATA( 27,27) ,MASS(3,2),L(3,2),RX(3,2),RY(3,2) ,RZ(3,2) 

D DIMENSION IXX(3,2),1IXZ(3,2),IXY(3,2),1YY(3,2),1YZ(3,2),122Z(3,2) 
D DIMENSION IMAT(3,3,3),NIMAT(3,3),TMAT(3,3), TMATTR(3,3),MATTMP( 3, 3) 
D DIMENSION LIMAT(3,3,3) 

FIXED  IER,I,J,M,K,P,N,IA,IDGT,A,I1 


ARRAY MATB(27),ABMATB( 27), LCOGX(3),LCOGY(3) , LCOGZ(3) 

ARRAY VECTA0(3) , VECTBO( 3), VECTA1(3) ,VECTB1(3) , VECTA2(3) , VECTB2(3) 
ARRAY VECTA(3) , VECTB(3) 

ARRAY WDX(3),WDY(3),WDZ(3),W1(3),W2(3) ,W3(3) 

ARRAY RBG1(3),RAG1(3) ,RBG2(3) ,RAG2(3) ,RBG3(3) 

ARRAY WKAREA( 850) 7 

ARRAY IXXT(3),IYYT(3) ,I1ZZT(3), IXYT(3) , IXZT(3) , 1YZT(3) 
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ARRAY DRCANX(3) ,DRCANY(3) , DRCANZ(3) 
ARRAY DRCSX(3) ,DRCSY(3) ,DRCS2Z( 3) 


INITIAL 

chit ta INPUT chataetteety 

7 INPUT PARAMETER CONSTANTS 

aD A = 15. 0D0 
Pa== 0 0D0 
W= 2 * PI 
IDGT = 6 
G = 9. 81D0 
N = 27 
M= 1 
IA = 27 
IER = 0 
LEVELQ = 0 
LEVLDQ = 0 

7 INPUT JOINT LOCATIONS IN METERS 
JXO = 0. ODO 
JYO = 0. ODO 
JZO = Q. ODO 
JX1 = 0. ODO 
JY) = 10,9000 
JZ1 = 0. 2D0 
JX2 = 0. ODO 
JY¥2 = 0.416D0 
242.— 0). Z)0 

ve INPUT TORQUE CONSTANTS 
TOX = 0. OD0 
Ory = 0. 0D0 
TOZ = 0. ODO 
T1x = 0. ODO 
ley: = 0. ODO 
IL = 0. 0D0 
2x = 0. ODO 
Za = 0. ODO 
Zz = 0. ODO 
TGA = 0. ODO 
TG2 = 0. ODO 
TIFNC = C. ODO 
T2ZFNC = 6. ODO 

¥ INPUT DISTANCE FROM CENTER OF LINK TO CENTER OF MASS 

¥ FOR EACH LINK ENDS 
L(1,1) = 0.05DO0O 
LCi, 2) = 0. 15D0 
L(2,1) = 0. 20D0 
L(2,2) = 0.216D0 
L(3, p= 0. 225D0 
L(3, 2p = 0. 226D0 


fell THESLINK GENGIHS OF THE ROBOT 
LINKL1 = 0. 20D0 
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ve 


120 


sc 


3 


ve 


INPUT MASS AT LINK ENDS IN KILOGRAMS 


OF LINK CENTERS OF GRAVITY 


INITIAL DIRECTION COSINES 


INPUT THE INITIAL DIRECTION COSINE ANGLES 


LINKL2 = 0.416D0 
LINKL3 = 0. 451D0 
MASS( 1.1) = 11-0be 
MASS(1,2) = 33. 0DO0 
MASS(2,1) = 2. 2D0 
MASS(2,2) = 2.2D0 
MASS(3,1) = 28. 6DO 
MASS(3,2) = 28. 6DO 
INPUT LOCATION 
LCOGK(1) = GyOno 
X1 = [COGX(1) 
LCOGY(1) = 0.O0DO 
Y1 = LCOGY(1) 
LCOGZ(1) = 0. 10D0 
Z1 = [LCOGZ(1) 
LCOGKX(2) = 0.0DO 
1G. = LCOGX(2) 
LCOGYG2) = 0. 208D0 
Nig = LCOGY(2) 
LCOGZ(2). = enon On 
Ta = [C0GZ(2) 
LCOGX(3) = 0.0DO0 
X3 =  LGOGXG3) 
LCOGY(3) = 0.6415D0 
Y3 = LCoOGcy(3) 
ECCGZ(3)1— aOeene 
ie = LCoGzZ3) 
INPUT MASS OF EACH LINK IN KG 
M1 = 44. ODO 
M2 = 4. 4D0 
M3 = 57.2D0 
INPUT ACCELERATIONS OF JOINT ZERO 
AXO = 0. 0DO 
AYO = 0. 0DO 
AZO = 0. ODO 
INPUT THE 
DRCSX(1) = 0. ODO 
DRCSY(1) = 0.0DO 
DRCSZ(1) = 1. 0D0 
DRCSX(2) = 0. ODO 
DRCSY(2) = 1. 0D0 
DRESZC2 = ONODO 
DRCSX(3) = 0. ODO 
DRCSY(3) = 1. ODO 
DRCSZ(3) = 0. ODO 
DRCANX(1) = 90. ODO 
DRCANY(1) = 90. ODO 
DRCANZ(1) = 0.0DO 
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DRCANK(2) = 90. ODO 
DRCANY(2) = 0. ODO 
DRCANZ(2) = 90. ODO 
DRCANX(3) = 90. ODO 
DRCANY(3) = 0. ODO 
DRCANZ(3) = 90. ODO 


vededesede INITIALIZE vedededede 


* OMEGA AND OMEGA DOT 

160 DGPI70 1 = 1,3 
W1¢T) = 0. 0D0 
W2(1) = 0.0D0 
W3( 1) = 0. ODO 
WDX(I) = 0. 0D0 
WDY( I) = 0. 0D0 
WDZ(1) = 0. 0D0 

bye) CONTINUE 

THZ = 0. O10 
& INITIALIZE MATRIX A AND B TO ZERO 


DO 180 I = 1,27 
MOPS J. = 1,27 
MATAC I,J) = 0. ODO 


175 CONTINUE 
MATB(I) = 0. ODO 
180 CONTINUE 


Citethrd te ee fs CALGUBATIONS Sevewerere 
vs WEIGHTS (NEWTONS) 


S5 WG1 = M1*G 
WG2 = M2*G 
WG3 = M3*G 
ve COMPUTE THE LENGTH FROM THE INBOARD JOINT TO COG 


LNCOG1 = DSQRT ( LCOGX(1)*LCOGX(1) + LCOGY(1)*LCOGY(1) +... 
LGOGZ(1)*leecaen) ) 


LX2 = LCOGX(2) - JX1 
LY2 = LCOGY(2) - JY1 
LZ2 = LCOGZ(2) - JZl1 


LNCOG2 = DSQRT ( LX2*LX2 + LY2*LY2 + LZ2*LZ2 ) 


LX3 = LCOGK(3) - JX2 
ie, — LCOCGY( 3) =) JY2 
LZ3 = LCOGZ(3) - JZ2 


LNCOG3 = DSQRT ( LX3*LX3 + LY3*LY3 + LZ3*LZ3 ) 


¥e COMPUTE INITIAL INERTIAS BASED ON POINT MASSES 
ve IN GLOBAL COORDINATES 
he 6) DO 225 I = 1,3 


MX(I,1) = -L(1I,1) * DRCSXCI) 


RX(I,2) = L(I,2) * DRCSX(I) 
RYCIe1) = -L(1,1) * DR@SY(I) 
RY(I,2) = L(I,2) * DRCSY(I) 
RZ(I,1) = -L(I,1) * DRCSZ(I) 
RZ(1,2) = L(1I,2) * DRCSZ(I) 


200 eer tye (CRYCI,1) “ "RY@iel)) + CRZC1,1) * R2(1,1))) 
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205 


225 


a 


ve 


1X?) 
IXxnen) 
IYY(I,1) 
Teael. 2) 
lyv nen) 
177i) 
L ZZ ) 
IARC ID) 
IXY(I,1) 
Elan) 
IXYT(I) 
als) 
Tcl 2) 
TXZnen) 
1YZe 1) 
TYZGne2) 
Ty Zen) 


IF CAA!) ee 


1.0.0 109) 
ELSE 

IXXT( 1) 
END IF 


ITE ( Yad) aE. 


LYYT(I) 
ELSE 

IYYT(I) 
END IF 


1B (OL /Au glig ad 
= .02 


IZZTC( I) 
ELSE 

IZZT(1) 
END IF 


MASS(I,2) * (CRY(I,2) 
TXXCb1) + Poco 
MASS(I,1) * ((RX(I,1) 
MASS(I,2) * ((RX(I,2) 
TYvCteiy, + Tyyer. 2) 
MASS(I,1) * ((RX(I,1) 
MASS(I,2) * ((RX(I,2) 
[Z7Che + Teen 
MASS(I,1) * RX(I,1) * 
MASS(I,2) * RX(I,2) * 
oC lawiy ce ISO Tage) 
MASS(I,1) * RZ(I,1) * 
MASS(I,2) * RZ(I,2) * 
TXZC (ion Paco) 
MASS(I,1) * RY(I,1) * 
MASS(I,2) * RY(I,2) * 
IYZ(I,1) + IY2(1,2) 
.02) THEN 

02 


19,6. 4G®) 


.02) THEN 
02 


me OO IL) 


702) CHEN 


WZ Zauncea) 


TRANSFORM THE 


IMAT(1,1,1) 
IMAT(i,1,2) 
IMAT(1,1,3) 
[MAT CID) 
IMAT(1I,2,2) 
IMAT(1,2,3) 
IMAT(1,3,1) 
IMAT(1,3,2) 
IMAT(1,3,3) 
CONTINUE 


i GE) 
Pan CL) 
IXZT(1) 
-1XYT(T) 
IYYT(I) 
LYZT(I1) 
-IXZT(I) 
-IYZT(I) 
ZA) 


rund db ue de ue a 


DUE TO LINK 
IXXT(1) 
Tsao) 
IXZT(1) 
LYYT(1) 
LY2T(1) 
TE Z7anel) 


IMAT(1,1,1) 
IMAT(1,1, 2) 
IMAT(1,1,3) 
IMAT(1,2,2) 
IMAT(1,2,3) 
IMAT(1,3,3) 


DO 9 I =2, 
TMAT( 2,1) 
TMAT( 2,2) 
TMAT( 2,3) 


| | 


0. ODO 


-DCOS ( THZ ) 
-DSIN ( THZ ) 


SO 


* 


* 
x 


a 
hy 


RY(1I,2)) 


RX(T,1)) 
RG 2) 


RG.) 
RCD 


RY(I,1) 
RY(1I,2) 


Rx, 1) 
RXier , 2) 


RA) 
RAGE 2) 


ve 


ae 


ee 
At 


(RZ(1, 2) 


CRAG tT) 
CRZClge2) 


(RY(I,1) 
(RY(I,2) 


INITIAL INERTIAS TO LOCAL COORDINATED 


* 


¥* 


* 


+ 


1 CONSTRAINTS LINK 1 INERTIAS ARE CONSTANT 


RA2(1,2))) 


RZ(1,1))) 
RZ(1,2))) 


RY(I,1))) 
RY(1,2))) 


TMAT(3,1) = DRCSX(I) 
TMAT(3,2) = DRCSY(I) 
TMAT(3,3) = DRCS2Z(I) 
VECTA(1) = TMAT(2,1) 
VECTA(2) = TMAT(2,2) 
VECTA(3) = TMAT(2,3) 
VECTB(1) = TMAT(3,1) 
VECTB(2) = TMAT(3,2) 
VECTB(3) = TMAT(3,3) 
CALL CPROD (VECTA,VECTB,MI1,MJ1,MK1) 
TMAT(1,1) = MII 
TMAT(1,2) = MJ1 
TMAT(1,3) = MK1 
TMATTR(1,1) = TMAT(1,1) 
TMATOUR( 1,2) = TMAT(2,1) 
TMATTR(1,3) = TMAT(3,1) 
TMATTR(2,1) = TMAT(1,2) 
TMATTR( 2,2) = TMAT(2,2) 
TMATTR(2,3) = TMAT(3,2) 
TMATTR(3,1) = TMAT(1,3) 
TMATTR(3,2) = TMAT(2,3) 
TMATTR(3,3) = TMAT(3,3) 
pO 5 11 = 1,3 
Ries) = 1,3 

TEMP = 0. ODO 

DO1 K =1,3 

TEMP = TMAT(I1,K) * IMAT(I,K,J) + TEMP 
1 CONTINUE 
MATTMP(I1,J) = TEMP 
5 CONTINUE 

Dome Il = Ie 
Dino J = 1.3 

TEMP = 0. ODO 


7 K eae 
ie = Ali r i kK) * TMARERCK, J) + TEMP 


y CONTINUE 
Poet (Cl. Lig) = TEMP 
8 CONTINUE 
Z CONTINUE 
DERIVATIVE 
NOSORT 
230 Grilmersobl (208,>256,-1,1,1) 
CALL UERSET( LEVELQ, LEVLDQ) 
* INITIALIZE MATRIX A AND B TO ZERO 


DO 240 I = 1,27 
memes5 J = 1,27 
MATA(I,J) = 0. 0DO 


ZO CONTINUE 

MATB(I) = 0. ODO 
240 CONTINUE 
* INPUT JOINT EQUATIONS 
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wv, 
“1 


JOINT ZERO EQUATIONS 


W1 X (W1 X RB/G1 
RBG1(1) = 
RBG1(2) = 
RBG1(3) = 
VECTA( 1) 
VECTA(2) 


) 

JIXO - eee) 
JYO - LCOGY(1) 
JZ0 - LCOGZ(1) 
W1(1) 

W1(2) 


VECTA(3) = W1(3) 
CALL CPROD( VECTAO,RBG1,MICO,MJCO,MKCO) 


VECTE Cir) 
VECTBO( 2) 
VECTBO0( 3) 


MICO 
MJCO 
MKCO 


CALL CPROD( VECTAO,VECTBO,MICO,MJCO,MKCO) 


INPUT JOINT EQUATIONS 
JOINT ZERO EQUATIONS 


W1 X (W1 X RB/G 
RBG1(1) 
RBG1(2) 
RBG1(3) 
VECTA( 1) 
VECTA( 2) 
VECTA(3) 

CALL CPROD(V 
VECTB(1) 
VECTB( 2) 
VECTB( 3) 

CALL CPROD(V 

W1 X (W1 X RA/G1 
RAG1(1) 
RAG1(2) 
RAG1(3) 
VECTA( 1) 
VECTA(2) 
VECTA(3) 

CALL CPROD ( 
VECTB( 1) 
VECTB( 2) 
VECTB(3) 

CALL CPROD ( 

W2 X (W2 X RB/G2 
RBG2(1) 
RBG2(2) 
RBG2(3) 
VECTA(1) 
VECTA( 2) 
VECTA(3) 

CALL CPROD ( 
VECTB(1) 
VECTB( 2) 
VECTB(3) 

CALL CPROD ( 

W2 X (W2 X RA/G2 
ROG 2m) 
RAG2(2) 


1) 

5X0 =" HeocxG 

JYO - LCOGY(1) 

JZ0 - LCOGZ(1) 
W1(1) 

W1(2) 
W1(3) 
ECTA,RBG1,MICO,MJCO,MKCO) 
MICO 

MICO 
= MKCO 
ECTA , VECTB , MICO ,MJCO ,MKCO) 
) 
JX1 - LCOGX(1) 
TYi. = EeOevGy 
JZ1 - LCOGZ(1) 

W1(1) 

W1(2) 

W1(3) 
VECTA ,RAG1 ,MIC1,MJC1,MKC1) 
MIC] 

MJC1 

MKC1 
VECTA, VECTB ,MIC1,MJC1 ,MKC1) 
) 
IXie=ceeeen@) 
JY1 - LCOGY(2) 
JZ =O OGZG2 ) 

W2(1) 

W2(2) 

W2(3) 

VECTA , RBG2 ,MIC2 ,MJC2,MKC2) 
= feo 
MJC2 
MKC2 
VECTA, VECTB ,MIC2 ,MJC2 ,MKC2) 
) 

JX2 - LCOGX(2) 

JY2 - LCOGY(2) 


RAG2(3) = JZ2 - LCOGZ(2) 
VECTA(1) = W2(1) 
VECTA(2) = W2(2) 
VECTA(3) = W2(3) 
CALL CPROD (VECTA, RAG2,MIC3,MJC3,MKC3) 
VECTB(1) = MIC3 
VECTB(2) = MJC3 
VECTB(3) = MKC3 
CALL CPROD(VECTA, VECTB,MIC3,MJC3,MKC3) 
cs W3 X (W3 X RB/G3) 
RBG3(1) = JX2 - LCOGX(3) 
RBG3(2) = J¥2 - LCOGY(3) 
REGI¢s) =ea2 = LCOGZ(3) 
VECTA(1) = W3(1) 
VECTA(2) = W3(2) 
VECTA(3) = W3(3) 
CALL CPROD (VECTA,RBG3,MIC4,MJC4,MKC4) 
VECTB(1) = MIC4 
VECTB(2) = MJC4 
VECTB(3) = MKC4 


CALL CPROD (VECTA, VECTB,MIC4,MJC4,MKC4) 


¥ INERTIA TRANSFORMATION 


pomecei = 2,5 

TMAT(2,1) = -DCOS ( THZ ) 
TMAT(2,2) = -DSIN ( THZ ) 
TMAIC?.3) = 0. 0D0 
TMAT(3,1) = DRCSX(I) 
TMAT(3,2) = DRCSY(I) 
TMAT(3,3) = DRCSZ(I) 
VECTA(1) = TMAT(2,1) 
VECTA(2) = TMAT(2,2) 
VECTA(3) = TMAT(2,3) 
VEGEEC1) = TMAE(3,1) 
VEGEBO2) = IMAT(3,2) 
VECTB(3) = TMAT(3,3) 
CALL CPROD (VECTA,VECTB,MI1,MJ1,MK1) 
TMAT(1,1) = MI1 
TMAT(1,2) = MJ1 
TMAT(1,3) = MK1 
MMATER( 1,1) = TWAP(1,1) 
TMATTR(1,2) = TMAT(2,1) 
TMATTR(1,3) = TMAT(3,1) 
TMATTR(2,1) = TMAT(1,2) 
TMATIR( 2,2) = TMAT(2,2) 
TMATTR( 2,3) = TMAT(3,2) 
TMATTR(3,1) = TMAT(1,3) 
TMATTR(3,2) = TMAT(2,3) 
TMATTR(3,3) = TMAT(3,3) 
DO 375 Il = 1,3 
DO 375 J =1,3 

TEMP = 0. ODO 

wems70 K = 1,3 
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3710. 


37) 


oo 


380 


390 


sesesiestese 


tat 
riwty 


vis 


TEMP = TMATTRC1I1,K) MAT EK, J) eee 


CONTINUE 
MATTMP(I1,J) = TEMP 
CONTINUE 
DO 48o 1) = ine 
DO 380°. = ie 
TEMP = 0. ODO 


DO 378K =e 
TEMP = MATIMPCT1 ,K) * TMATGR SS yaoi 


CONTINUE 
NIMAT(I1,J) = TEMP 
CONTINUE 
IXXT(I) = NIMAT(1,1) 
IXYT(I) = NIMAT(1,2) 
IXZT(I) = NIMAT(1,3) 
IYYT(I) = NIMAT(2,2) 
LIYZT(I) = NIMAT(2,3) 
IZZT(I) = NIMAT(3,3) 
CONTINUE 


ENTER CONSTANTS INTO MATRIX A AND B_ *¥2ereve 
LINK ONE 

SUM OF FORCES 

X DIRECTION 


MATA(1,1) = 1.0D0 
MATA(1,4) = Ml 
MATA(1,10) = -1.0D0 
MATB(1) = 0.0DO 
Y DIRECTION 
MATA(2,2) = 1.0D0 
MATA(2,5) = M1 
MATA(2,11) = -1.0D0 
MATB( 2) = 0.0D0 
Z DIRECTION 
MATA(3,3) = 1.0D0 
MATA(3,6) = Ml 
MATA(3,12) = -1.0DO0 
MATB(3) = -WGl 


EQUATIONS AT JOINT ZERO 
X DIRECTION 


MATA(4,4) = 1. 0D0 
MATA(4,8) = RBG1(3) 
MATA(4,9) = -RBG1(2) 
MATB(4) = AXO - MICO 
Y DIRECTION 
MATA(5,5) = 1.0D0 
MATA(5,7) = -RBG1(3) 
MATA(S,9) = RBG1(1) 
MATB(5) = AYO - MJCO 
Z DIRECTION 
MATA(6,6) = 1.0D0 
MATA(6,7) = RBG1(2) 
MATA(6,8) = -RBG1(1) 


S4 


Jc 


a V5 


ses 


Yd 


460 


Ay 


+ 


X* 


MATB(6) = AZO - MKCO 
SUM OF MOMENTS EQUATIONS 
X DIRECTION 

MATA(7,2) = RBG1(3) 

MATA(7,3) = -RBG1(2) 

MATA(7,7) = -IXXT(1) 

MATA(7,8) = IXYT(1) 

MATA(7,9) = IXZT(1) 

MATA(7,11) = -RAG1(3) 

MATA(7,12) = RAG1(2) 

MATB(7) = TIX - TOX 
Y DIRECTION 

MATA(8,1) = -RBG1(3) 

MATA(8,3) = RBG1(1) 

MATA(8,7) = IXYT(1) 

MATA(8,8) = -IYYT(1) 

MATA(8,9) = IY2ZT(1) 

MATA(8,10) = RAG1(3) 

MATA(8,12) = -RAG1(1) 

MATB(8) = T1Y - TOY 
Z DIRECTION 

MATA(9,1) = RBG1(2) 

MATAC9,2) = -RBGI(1) 

MATA(9,7) = IXZT(1) 

MATAC9,8) = IY2ZT(1) 

MATA(9,9) = -IZ2T(1) 

MATA(9,10) = -RAGI(2) 

Memeccet) = RAGI(1) 

MATB(9) = T1Z - TOZ 
LINK TWO 
SUM OF FORCES 
X DIRECTION 

MATA(10,10) = 1.0D0 

MATA(10,13) = M2 

MATA(10,19) = -1. 0DO 

MATB( 10) = 0.0D0 
Y DIRECTION 

MATA( 11,11) = 1.0D0 

MATA(11,14) = M2 

MATA(11,20) = -1.0D0 

MATB( 11) = 0.0D0 
Z DIRECTION 

MATA(12,12) = 1.0D0 

MATA( 12,15) = M2 

MATA( 12,21) = -1.0D0 

MATB( 12) = -WG2 
EQUATIONS AT JOINT ONE 
X DIRECTION 

MATA(13,4) = -1.0D0 

MATA(13,8) = -RAG1(3) 

MATA(13,9) = RAG1(2) 

MATA( 13,13) = 1.0D0 

MATA( 13,17) = RBG2(3) 

MATA( 13,18) = -RBG2(2) 

MATB( 13) = MIC1 - MIC2 
Y DIRECTION 
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ae 


So10, 


ve 


MATA(14.59 §= =alaoin@ 
MATA(14,7) = RAG1(3) 
MATA(14,9) = -RAG1(1) 
MATA(14,14) = 1.0D0 
MATA(14,16) = -RBG2(3) 
MATA(14,18) = RBG2(1) 
MATB( 14) = MJC1 - MJC2 

Z DIRECTION 
MATA(15,6) = -1.0D0 
MATA( 15,7) = -RAG1(2) 
MATA(15,8) = RAG1(1) 
MATA(15,15) = 1.0D0 
MATA(15,16) = RBG2(2) 
MATA( 15,17) = -RBG2(1) 
MATB( 15) = een = MKC2 

SUM OF MOMENTS EQUATIONS 

X DIRECTION 
MATA(16,11) = RBG2(3) 
MATA(16,12) = -RBG2(2) 
MATA( 16,16) = -IXXT(2) 
MATA( 16,17) = IXYT(2) 
MATA( 16,18) = IXZT(2) 
MATA(16,20) = -RAG2(3) 
MATA( 16,21) = RAG2(2) 
MATB( 16) = TOX -"Tix 

Y DIRECTION 
MATA(17,10) = -RBG2(3) 
MATA( 17,12) = @RBG2(1) 
MATA(17,16) = IXYT(2) 
MATA(17,17) = -IYYT(2) 
MATA(17,18) = IYZT(2) 
MATA(17,19) = RAG2(3) 
MATA(17,21) = -RAG2(1) 
MATB(17) = T2Y - T1Y 

Z DIRECTION 
MATA(18,10) = RBG2(2) 
MATA(18,11) = -RBG2(1) 
MATA(18,16) = IXZT(2) 
MATA(18,17) = IYZT(2) 
MATA( 18,18) = -IZ2ZT(2) 
MATA(18,19) = -RAG2(2) 
MATA( 18,20) = RAG2(1) 
MATB( 18) == eZ 

LINK THREE 

SUM OF FORCES 

X DIRECTION 
MATA(19,19) = 1.0D0 
MATA(19,22) = M3 
MATB( 19) = 0, 0D0 

Y DIRECTION 
MATA(20,20) = 1.0D0 
MATA(20,23) = M3 
MATB( 20) = 10) cin 

Z DIRECTION 
MATA(21,21) = 1.0D0 
MATA(21,24) = M3 
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ities 
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D0 


ee 


MATB(21) = -WG3 
EQUATIONS AT JOINT TWO 


X DIRECTION 
MATA( 22,13) 
MATA( 22,17) 
MATA( 22,18) 
MATA( 22,22) 
MATA( 22,26) 
MATA( 22,27) 
MATB( 22) 

Y DIRECTION 
MATA( 23,14) 
MATA( 23,16) 
MATA( 23,18) 
MATA( 23,23) 
MATA( 23,25) 
MATA( 23,27) 
MATB( 23) 

Z DIRECTION 
MATA( 24,15) 
MATA( 24, 16) 
MATA( 24,17: 
MATA( 24,24) 
MATA( 24,25) 
MATA( 24,26) 
MATB( 24) 


-1. ODO 
-RAG2(3) 
RAG2(2) 

1. ODO 
RBG3(3) 
-RBG3(2) 
MIC3 - MIC4 


-1. ODO 
RAG2(3) 
-RAG2(1) 

1. ODO 
-RBG3(3) 
RBG3( 1) 
MJC3 - MJC4 


-1. ODO 
-RAG2(2) 
RAG2( 1) 

1. ODO 

RBG3( 2) 
-RBG3(1) 
MKC3 - MKC4 


SUM OF MOMENTS EQUATIONS 


X DIRECTION 
MATA( 25,20) 
MATA( 25,21) 
MATA( 25,25) 
MATA( 25, 26) 
MATA( 25, 27) 
MATB( 25) 

Y DIRECTION 
MATA( 26,19) 
MATA( 26,21) 
MATA( 26,25) 
MATA( 26,26) 
MATA( 26, 27) 
MATB( 26) 

Z DIRECTION 
MATA( 27,19) 
MATA( 27, 20) 
MATA( 27,25) 
MATA( 27, 26) 
MATA( 27 ,27) 
MATB( 27) 


RBG3(3) 
-RBG3(2) 
=TXXT(3) 

IXYT(3) 

Iz) 
-T2X 


-RBG3(3) 
RBG3( 1) 
IXYT(3) 

-IYYT(3) 
iD VAC) 

-T2Y 


RBG3(2) 

-RBG3(1) 
TXAR(S;) 
IYZT(3) 
-1Z2T( 3) 
-T22Z 


FIRST LINK IS CONSTRAINED TO ROTATE IN Z DIRECTION 


MODIFIED BY RM VERBOS 
DO 600 I = 4,8 
DO 595 J = 1,27 


MATA(I,J) = 0. ODO 
MATA(J,I) = 0. 0DO 
CONTINUE 


Syl 


sKveveclese 


600 


605 


S119, 


* 
620 


624 


627 


630 


641 


625 
bo) 


seveo'e 


Jc 
640 


“* 


*660 


MATB(I) = 0. ODO 
CONTINUE 
MATA(4,4) = 1. 0D0 
MATA(5,5) = 1. 0DO 
MATA(6,6) = 1. 0DO 
MATA(7,7) = 1. 0D0 
MATA(8,8) = 1. 0DO 
DOoredos) — aay 
MATA(18,J) = 0. 0DO 
MATA(27,J) = 0. ODO 
CONTINUE 
MATA(9,9) = -(IZZT(1)+1ZZT(2)+1Z2ZT(3)) 
MATA(18,9) = -1.0D0 
MATA(18,18) = 1.0D0 
MATB( 18) = 0.0D0 
MATA(27,9) = -1.0DO 
MATA( 27,27) = 1.0DO 
MATB( 27) = @.0D0 


CALL EQUATION SOLVER PROGRAM FROM IMSL 
CALL LEQT2F(MATA,M,N,IA,MATB, IDGT,WKAREA, IER) 
IF (IER. EQ. 0) THEN 
GOTO 640 
ELSE 
WRITE (*,624) IER 
FORMAT (IS) 
DO 635. 1 = ieee? 
WRITE (*,627) I 
FORMAT (17) 
DO 631 J=1, 27, 3 
WRITE (*,630) J,MATA(I,J),J+1,MATA(I,J+1) ,J+2,MATA(I,J+2) 
FORMAT (15,F13.5,15,F13.5,15,F13. 5) 
CONTINUE 
WRITE (*,633) I,MATB(I) 
FORMAT (13,F15. 5) 
CONTINUE 
CALL ENDJOB 
END IF 


FIND LCOGX, LCOGY , LCOGZ,THETA VALUES ,WX,WY,WZ 


JOINT ZERO 
FXO = MATB(1) 

FYO = MATB(2) 

FZ0 = MATB(3) 

LINK ONE 

SINCE LINK1 IS CONSTRAIN TO ROTATE AROUND THE Z ONLY 
AX1 = 0. 0DO 

AY1 = 0. 0DO 

AZ1 = 0.0D0 

AX1 

VELX1 

LCOGX1 

LCOGX(1) 


MATB(4) 
INTGRL(0. ,AX1) 
INTGRL(X1, VELX1) 
LCOGX1 
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sc 
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sc 
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Scocc 
685 


veve 


740 


AY1 = MATB(5) 
VELY1 = INTGRL(0. ,AY1) 
more nal = INTGRL(Y1,VELY1) 
LCOGY(1) = LCOGY1 
AZ1 = MATB(6) 
VELZ1 = INTGRI(0. ,AZ1) 
mega7 1 = INTGRL(Z1,VELZ1) 
LCOGZ(1) = 1C0GZ1 

WD1X = MATB(7) 

W1X = INTGRL(0. ,WD1X) 

WDX(1) = WD1X 

W1(1) = W1X 

WD1Y = MATB(8) 

W1Y = INTGRL(0. ,WD1Y) 

WDY(1) = WD1Y 

W1(2) = W1Y 

WD1Z = MATB(9) 

W1Z = INTGRL(0. ,WD1Z) 

WDZ(1) = WD1Z 

W1(3) = Wa 


Sours bY Ro Maven DOs 


THZ = INTGRL(O. ,W1Z) 
THZANG = THZ * RADEG 
COSTHZ: = DCOS(THZ) 
SINTHZ = DSINCTHZ) 


Vee tnE TS EINK Is CONSIN@MN TO KOTATE IN THE 2 DIRECTION ONLY 
THE DIRECTION COSINE AND DIRECTION COSINE ANGLES ARE CONSTANT 
AND DO NOT NEED TO BE CALCULATED 


CALC DIRECTIONAL COSINES FOR LINK ONE 


DRCSX(1) = LCOGX1 / LNCOG1 
DRCSY(1) = LCOGY1 / LNCOG1 
DRCSZ(1) = LCOGZ1 / LNCOG1 

AMP = DSQRT(DRCSX(1)**DRCSX(1)+DRCSY(1)*DRCSY(1)+... 

DRCSZ(1)**DRCSZ(1)) 

DRCSX(1) = DRCSX(1)/AMP 
DRCSY(1) = DRCSY(1)/AMP 

DRCSZ(1) = DRCSZ(1)/AMP 

DRCANX(1) = DACOS(DRCSX(1)) * RADEG 
DRCANY(1) = DACOS(DRCSY(1)) * RADEG 
DRCANZ(1) = DACOS(DRCSZ(1)) * RADEG 


JOINT LOCATION 


JX1 = LINKL1 * DRCSX(1) 
JY1 = LINKL1 * DRCSY(1) 
JZ1 = WLINKL1 * DRCSZ(1) 
JOINT ONE 
FX1 = MATB(10) 
FY1 = MATB(11) 
FZ1 = MATB( 12) 
LINK TWO 
AX2 = MATB(13) 
VELX2 = INTGRL(0. ,AX2) 
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800 


* 
805 
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812 


EGOGH:2 = INTGRL(X2,VELX2) 
LCOGKO) eee 
AY2 = MATB(14) 
VELY2 = INTGRL(0. ,AY2) 
LCOGY2 = INTGRL(Y2,VELY2) 
LCOGY(2) = ieee 
AZ2 = MATB(15) 
VELZ2 =) TNIGRECOMSaZe) 
Ie OG AD = INGeRnG@Z2 VE 
LCoez(2) = eee 
WD2X = MATB( 16) 
W2X = INTGRL(0. ,WD2X) 
WDX(2) = WD2X 
W2(1) = W2x 
WD2Y = MATB(17) 
W2Y = INTGRL(0. ,WD2Y) 
WDY(2) many a 
W2(2) = W2Y 
WD2Z = MATB(18) 
W2Z = INTGRL(0. ,WD2Z) 
WDZ(2) = ney 
W2(3) =O, 
GET THE DIRECTION COSINES FOR THE LINK TWO 
DROSX(2) = (LGOGKX2 - JX1) / INGGGZ 
DR@SY(2) = (LCOGWONe JY1) /SiNcoe? 


DRCSZ(2) = (LCOGZ2 - JZ1) / LNCOG2 


AMP = DSQRT(DRCSX(2)*DRCSX(2)+DRCSY(2)*DRCSY(2)+... 


DRCSZ(2)*DRCSZ( 2) ) 


DRCSX(2) = DRCSX(2)/AMP 
DRCSY(2) = DRCSY(2)/AMP 
DRCSZ(2) = DRCSZ(2)/AMP 
DRCANX(2) = DACOS(DRCSX(2)) * RADEG 
DRCANY(2) = DACOS(DRCSY(2)) * RADEG 
DRCANZ(2) = DACOS(DRCSZ(2)) * RADEG 


JOINT LOCATION 


JX2 = JX1 + LINKL2 * DRCSX(2) 

JY¥2 = QYi + LINKL2 * DRGs CZ) 

JZ2 = JZ1 + LINKL2 * DRCSZ(2) 
JOINT TWO 


FX2 = MATB(19) 
FY2 = MATB(20) 
FZ2 = MATB(21) 


LINK THREE 
AX3 = MATB( 22) 
VELX3 = INTGRL(0. ,AX3) 
LOOGxS = INTGRL(X3, VELX3) 
MeOCN( S)e= Neoers 
AY3 = MATB(23) 
VELY3 = INTGRL(0. ,AY3) 
LCOGY3 = INTGRL(Y3,VELY3) 
LGOGY(3) = Eeocws 
AZ3 = MATB( 24) 
VELZ3 = INTGRL(0. ,AZ3) 
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LCOGZ3 = 

LCOGZ(3) = LCOGZ3 
WD3X = MATB( 25) 
W324 = 

WDX(3) = WD3xX 
W3(1) = W3X 
WD3Y = MATB( 26) 
W3Y = 

WDY( 3) = WD3Y 
W3(2) = W3Y 
WD3Z = MATB( 27) 
W3Z = 

WDZ(3) = WD3Z 
W3(3) = W3Z 


* CALC DIRECTIONAL COSINES FOR LINK THREE 

845 DRCSX(3) = (LCOGX3 - JX2) / LNCOG3 
DRGsyes) = (LCOGY3 - JY2) / LNCOGS 
DRCSZ(3) = (LCOGZ3 - JZ2) / LNCOG3 


INDEGRLC 225 VELZay 


INTGRL(CO. ,WD3X) 


INTGRL(O. ,WD3Y) 


INTGRL(O. ,WD3Z) 


865 AMP = DSQRT( DRCSX(3)*DRCSX(3)+DRCSY(3)*DRCSY(3)+... 
DRCSZ(3)**DRCSZ(3)) 
DRCSX(3) = DRCSX(3)/AMP 
DRCSY(3) = DRCSY(3)/AMP 
DRCSZ(3) = DRCSZ(3)/AMP 
DRCANX(3) = DACOS(DRCSX(3)) * RADEG 
DRCANY(3) = DACOS(DRCSY(3)) * RADEG 
DRCANZ(3) = DACOS(DRCSZ(3)) * RADEG 


* TIP LOCATION 


875 TIPX = 
TIPY = 
TPZ = 

* FIND THE 

880 ANG23X = DRCANX(2) - 
ANG23Y = DRCANY(2) - 
ANG23Z = DRCANZ(2) - 
ANG12X = DRCANX(1) - 
ANG12Y = DRCANY(1) - 
ANG12Z = DRCANZ(1) - 

DYNAMIC 

NOSORT 

vevterets ~=INPUT TORQUE AT JOINTS 


JX2 + LINKL3 * DRCSX(3) 
JY2 + LINKL3 * DRCSY(3) 
JZ2 + LINKL3 * DRCSZ(3) 


ANGLE BETWEEN THE LIMKS 


DRCANX(3) 
DRCANY( 3) 
DRCANZ(3) 


DRCANX( 2) 
DRCANY(2) 
DRCANZ(2) 


* CALCULATE THE MAGNITUDE OF THE LENGTH OF THE PROJECTION OF 
* LINK 2 AND 3 IN THE X Y PLANE WHICH IS THE MOMENT ARM FOR 


% THE CALCULATION OF THE GRAVITATIONAL TORQUE 


900 MARM2A 
MARM2B 
MARM3 


Wow ou 


DSQRT ( LCOGK2 * LCOGX2 + LCOGY2 
DSQRT ( LCOGX3 * LCOGX3 + LCOGY3 
DSQRT ( CLCOGX3 - JX2) * (CLCOGX3 
(CLEOGYS = JY2) * (Leocy3 


* CALCULATE GRAVITATIONAL TORQUES 
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MARM2A * WG2 + MARM2B * WG3 
MARM3 * WG3 


* INPUT TORQUES OVERIOP SOR SCRA iy 


*10 TIFNC = 2*A - A * DCOS ( W * TIME ) 
cb T2ENC = 3-355 985% DSUN@ CW ahiiED 
* TOZ = A * DSIN ( W * TIME ) 
* TOZ = A -A* DSIN ( W * TIME ) 
* TOZ = 50.0 =35070 = 2c 
* CALCULATE TOTAL TORQUE ON JOINT 1 AND 2 
Tl = TGl + Titec 
12 = 1G2 een 


* CALCULATE X AND Y COMPONENT OF TORQUE ON JOINT 1 AND 2 
T1X = T1 * DCOS (THZ) 


T1Y = T1 * DSIN (THZ) 

T2X = T2 “700s (Ti 

T2Y = T2 * SIN (THZ) 
* TORQUE CHECK 


TICH = DSQRT ( TixX * T1xX + 1 {ies 
T2CH = DSQRT ( 12k * 12K + Tai ee, 


END 
SOr 


FORTRAN 
% SUBROUTINE TO COMPUTE THE CROSS PRODUCT OF TWO VECTORS 


SUBROUTINE CPROD( VECTA, VECTB ,MI ,MJ,MK) 
940 IMPLICIT REAL*8 (A-Z) 
DIMENSION VECTAC 3 RV ECIRGs 
MI = VECTAC2) * VECTB(3) - VECTA(3) * VECTB(2) 
MJ = VECTA(3) * VECTB(1) - VECTA(1) * VECTB(3) 
MK = VECTAC1) * VECTB(2) - VECTA(2) * VECTB(1) 
RETURN 


END 


62 


APPENDIN B 


DOUBLE PENDULUM VALIDATION PROGRAM 


ve 
¥ 
ve 
rAd 
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TITLE DOUBLE PENDULUM VALIDATION PROGRAM 

Jedodetesedetede tevede fede teteve cede te ted Ree RRR RR aie tecesetetesesetokKRsekesedteteke se fe tete eek FR RIERA 
* THIS IS A PROGRAM THAT WILL SOLVE THE DIRECT DYNAMICS PROBLEM FOR A 

* 2 LINK RIGID BODY MANIPULATOR WITHOUT THE USE OF THE STANDARD TRAN- 

* FORMATION MATRICES AND VALIDATE THE MOTION BY THE USE OF THE DOUBLE 

* PENDULUM PROBLEM. 

* liebe y EL RE Os USN 

SS rr ein de air de tay ter str oh oie bie Lake RY ok Le ie bee Se eae bie De aca ar ke bark ae ti boar ck ace ALR ERROR Re Re OP TR 
TERMINAL 


METHOD ADAMS 


PRINT 


CONTROL FINTIM =4.0, DELT = .0005, DELMAX = .1, DELPRT = 
-05, ANGTH2 , TH2ANG , ANGTH3 , TH3ANG, ANG23 


SAVE 
GRAPH 
GRAPH 
GRAPH 
D 

D 

D 

D 
FIAED 
ARRAY 
ARRAY 
ARRAY 
ARRAY 
ARRAY 
ARRAY 
ARRAY 
ARRAY 
ARRAY 


10, ANGTH2 , TH2ANG , ANGTH3 , TH3ANG , ANG23 
10 


(DE=TEK618) TIME, ANGTH2 , TH2ANG 
(DE=TEK618) TIME,ANGTH3, TH3ANG 
(DE=TEK618) TIME,ANG23 
DIMENSION MATA( 27,27) ,MASS(3,2),L(3,2),RX(3,2) .RY(3,2),RZ(3,2) 
DRE NS Wane ixx( 3.2). 182( 3,2), IXY(3,2),1YY(3,2),1YZ(3,2),1Z2(3,2) 
DIMENSION IMAT(3,3,3),NIMAT(3,3),TMAT(3,3),TMATIR(3,3),MATINP( 3,3) 
DIMENSION LIMAT(3, 3,3) 

TERGGenOk,P,N,IA,1DGT,A,T1 
MATB( 27) ,ABMATB( 27) , LCOGX(3) , LCOGY(3) , LCOGZ(3) 
VECTAO(3) , VECTBO(3), VECTA1(3) ,VECTB1(3) , VECTA2(3) , VECTB2(3) 
VECTA( 3) , VECTB(3) 
WDX(3) ,WDY(3) ,WDZ(3),W1(3) ,W2(3) ,W3(3) 
RBG1(3) ,RAG1(3) ,RBG2(3),RAG2(3) ,RBG3(3) 
WKAREA( 850) 
pea syivean(s) , 122003), 1XYT(3),1XZT(3) , 1YAuC3) 
DRCANX(3) , DRCANY( 3) , DRCANZ(3) 
DRCSX( 3), DRCSY(3) , DRCSZ(3) 


INITIAL 


Sevededede 
se 
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deveve 


* 


Setevevesde 


PARAMETER CONSTANTS } 
15. ODO : 
P = 0. ODO 
W’ Za PI 
IDGT = 6 
MODIFIED BY RM VERBOS 
G Jao D0 
G = 0. 0DO0 
N 24 


INPUT 
INeea 
A 
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= 2 


TA = 27 
IER = 0 
LEVELQ = 0 
LEVLDQ = 0 
% INPUT JOINT LOCATIONS IN METERS 
JXO = 0. ODO 
JYO = 0.0D0 
JZO = 0. 0D0 
JX] = 0.006 
JY1 = 0. 0D0 
JZ1 = 0. 2D0 
JX2 = 0.010 
JY2 = 0.416D0 
JZ2 = 0.2D0 
x INPUT TORQUE CONSTANTS 
TOX = 0.0D0 
TOY $= 0.0D0 
TOZ # = 0.0D0 
TIX = 0.0D0 
T1IY = 0.0D0 
T1Z = 0.0D0 
T2X = 0.0D0 
T2Y = 0.0D0 
Tog = 0. 0D0 
INPUT DISTANCE FROM CENTER OF LINK TO CENTER OF MASS 
ve FOR EACH LINK ENDS 
L(1,1) = 0. 05D0 
L(1,2) = 0.15D0 
L(2,1) = 0. 20D0 
L(2,2) = 0.216D0 
L(3,1) = 0. 225D0 
L(3,2) = 0.226D0 
** INPUT THE LINK LENGTHS OF THE ROBOT 
LINKL1 = 0. 20D0 
LINKL2 = 0.416D0 
LINKL3 = 0.451D0 
ve INPUT MASS AT LINK ENDS IN KILOGRAMS 
110 MASS(1,1) = 11. 0D0 
MASS(1,2) = 33. ODO 
MASS(2,1) = 2.2D0 
MASS(2,2) = 2.2D0 
MASS(3,1) = 28. 6DO 
MASS(3,2) = 28. 6DO 
“e INPUT LOCATION OF LINK CENTERS OF GRAVITY 
120 LCOGX(1) = 0.6D0 
X1 = 1ecewe) 
LCOGY(1) = 0.600 
Yl = LCOGY(1) 
LCOGZ(1) = 0.10D0 
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ol = LC0GZ(1) 
LCOGX(2) = 0. O0DO 
X2 = LCOGX(2) 
LCOGY(2) = 0. 208D0 
eZ = LCOGY(2) 
LCOGZ(2) = 0. 20D0 
lip = LCOGZ(2) 
LCOGX(3) = 0.O0DO 
KS = LCOGX(3) 
LCOGY(3) = 0.6415D0 
acs = LCcOGY(3) 
LCOGZ(3) = 0. 2D0 
23 = LC0GZ(3) 
* INPUT MASS OF EACH LINK IN KG 
Mi = 44. 0D0 
M2 = 4. 4D0 
Mae= 57. Z2)0 
* INPUT ACCELERATIONS OF JOINT ZERO 
AXO = 0. ODO 
AYO = 0. 0D0 
AZO = Q. ODO 
¥ INPUT THE INITIAL DIRECTION COSINES 
DRCSX( 1) = 0. ODO 
DRCSY(1) = 0. ODO 
PRoocet) =i. 000 
DRCSK(2) = 0. ODO 
DRCSY(€2) = 1. 0D0 
DRCSZ(2) = 0. ODO 
DRCSX(3) = 0. ODO 
DRCSY(C3) = 1. 0D0 
DRCSZ(3) = 0. 0DO 
¥ INPUT THE INITIAL DIRECTION COSINE ANGLES 
DRCANX( 1) = 90. ODO 
DRCANY(1) = 90. 0DO 
DRCANZ( 1) = 0. ODO 
DRCANX(2) = 90. ODO 
DRCANY(2) = 0. ODO 
DRCANZ(2) = 90. ODO 
DRCANX(3) = 90. 0DO 
DRCANY(3) = 0.ODO 
DRCANZ(3) = 90. ODO 
Hiewaede sr INIT Pac ZE Chae itor lily 
* OMEGA AND OMEGA DOT 
160 DO 170 I = 1,3 
WiCI) = 0. ODO 
W2(1) = Q. ODO 
W3C1) = 0. ODO 
WDX(T) = 0. ODO 
WDYCI) = Q. ODO 
WDZ(I) = 0.0D0 
170 CONTINUE 


180 


Seveseoese 


chy 


185 


* 


200 


Zo 


THZ 


= Q. 0D0 


INITIALIZE MATRIX A AND B TO ZERO 
po 160 1 — a7 
DO 175 J = 1,27 


MATAC I,J) = 0. ODO 


CON Tah vr 


MATB(I) = 0. ODO 
CONTINUE 


CALCULATIONS 


WEIGHTS 
WG1 
WG2 
WG3 


COMPUTE 


LxX2 
LY2 
LZ2 


LY3 


(NEWTONS ) 


M1*G 
M2*G 
M3*G 


THE LENGTH 
LNCOG1 = DSQRT ( 


LCOGX(2) - 
LCOGY(2) - 
LCOGZ(2) - 
LNCOG2 = DSQRT ( 
LX3 = LCOGR(3) - 
= HCO0GH(3)-* 
[23 = eoeaes) 
LNCOG3 = DSORT ( 


sevesk ses 


LCOGZ(1)*LCOGZ(1) ) 


JX1 
seal 
zal 
JXK2 


Jir2 
eZ 


FROM THE INBOARD JOINT TO COG 
LCOGX(1)*LCOGK(1) + LCOGY(1)*LCOGY(1) +... 


LA2*X2 + LYZeLY2 Pen Z2 72) 


LX3*LX3 + LYS*hY¥o aaa 3 coe 


COMPUTE INITIAL INERTIAS BASED ON POINT MASSES 
IN GLOBAL COORDINATES 
aes 


DO 


Tie 
IXX(1I,2) 
IXXDG@n 
IYY(I,1) 
IYY(I,2) 
neared 
TZ 7a) 
UAC. 2D) 
1270) 
IXY(I,1) 
pov Dy 
SSAC 
IZ) 
CAC oe 
1XZTGm) 
IYZ(I,1) 
TY ZG) 
IYZT(1) 


225 I = 
RXCIA 1) 
RX(1, 2) 
RY(1,*) 
RY(I,Z) 
RZ(1,1) 
RZ(1,2) 


=LE( 1,1) 
rel; 2) 
mC 12) 
lifel , 2) 
=Als leer) 
LCI 42 


DRCSX( IT) 
DRCSX( 1) 
DRCSY( 1) 
DRCSY( 1) 
DRCSZ( 1) 
DRCSZ( TI) 


MASS(I,1) * ((RY(I,1) * 
MASS(I,2) * ((RY(I,2) * 
Eacien) + Ix) 
MASS(I,1) * ((RX(I,1) * 
MASS(1I,2) * ((RX(I,2) * 
TYY(I,1) + IYY(I,2) 
MASS(I,1) * ((RX(I,1) * 
MASS(1I,2) * ((RX(I,2) * 
ACLS) Com oa) 
MASS(I,1) * RX(I,1) * 
MASS(1,2) * RX(I,2) * 
IXY(I,1) + IXY(I,2) 
MASS(I,1) * RZ(I,1) * 
MASS(I,2) * RZ(1,2) * 
TXZ( De (1, 2) 
MASS(I,1) * RY(I,1) * 
MASS(1I,2) * RY(I,2) * 
TYZ(1 YG 2) 
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Rel) 
RY(I,2)) 


RX(1I,1)) 
RAC I,2)) 


RX(I,1)) 
RX(I,2)) 


oA Py) 
1 Oi i ig’) 


RWG@Ly1) 
RA Gi) 


RZ( 1,1) 
RZGr 2) 


+ 


+ 


ae 


(R2Z(1,1) 
(RZ(1,2) 


(R2Z2(I,1) 
(RZ(1, 2) 


(RY(1,1) 
CRuCI 2) 


+ 


* 


* 


+ 


RY( 
RY(1I,2))) 


IF (IXXT(I) . LE. .01) THEN 

PoXTCI) =o 

ELSE 

mox( 1) = Exxt(1) 

END IF 

IF (IYYT(1) . LE. .02) THEN 

IYYT(1) = .02 

ELSE 

ay ( 1s = yen 1) 

END IF 

IF (IZZT(1) . LE. .02) THEN 

aaZT( te 900 

ELSE 

2c ijee= 12201) 

END IF 
IMAT(I,1,1) 
IMAT(1I,1,2) 
IMAT(I,1,3) 
IMAT(I,2,1) 
IMAT(I,2,2) 
IMAT(1I,2,3) 
IMAT(I,3,1) 
Toate 3.2 ) 
IMAT(I,3,3) 

225 CONTINUE 


ox (1) 
eyT(1) 
1GALCIO) 
~IXYT(I) 
IYYT(I) 
ieeZ¢ 1) 
~IXZT(I) 
-IYZT(1) 
ETA 


Hud uu weue a a 


% DUE TO LINK 1 CONSTRAINTS LINK 1 INERTIAS ARE CONSTANT 
1X eMAT( 110 11) 
Ty —mnATC 1 1,2) 
IXZT(1) = IMAT(1,1,3) 
IYYT(1) = IMAT(1,2,2) 
TYZu@iye— IMAT(1,2,3) 
[ZZ IAT 13) 3) 


Hou ud ud ue a 


* TRANSFORM THE INITIAL INERTIAS TO LOCAL COORDINATED 
be 9 Ie 

TMAT( 2,1) 

TMAT( 2,2) 

TMAT( 2,3) 

TMAT( 3,1) 

TMAT( 3,2) 

TMAT(3,3) 


Go 


-DGOs+( THZ ) 
=o THZ ) 
0. ODO 
DRCSX( I) 
DRCSY(T) 
DRCSZ(1) 


VECTA( 1) 
VECTA( 2) 
VECTA( 3) 
VECTB( 1) 


TMAT( 2,1) 
TMAT( 2,2) 
TMAT( 2,3) 
TMAT( 3,1) 
VECTB( 2) TMAT( 3,2) 
VECTB(3) TMAT( 3,3) 
CALL CPROD (VECTA,VECTB,MI1,MJ1,MK1) 
TMAT(1,1) = MI1 
TMAT(1,2) = MJ1 
TMAT(1,3) = MK1 
TMATTR(1,1) = TMAT(1,1) 
TMATTR(1,2) = TMAT(2,1) 
TMATTR(1,3) = TMAT(3,1) 


lou il 
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TMATTR(2,1) = TMAT(1,2) 
TMATTR(2,2) = TMAT(2,2) 
TMATTR(2,3) = TMAT(3,2) 
TMATTR(3,1) = TMAT(1,3) 
TMATTR(3,2) = TMAT(2,3) 
TMATTR(3,3) = TMAT(3,3) 
DO 5 Il = 1,3 
DO5 J =1,3 
TEMP = 0. ODO 
D011 K =1,3 
TEMP = TMAT(11,K) * IMAT(I,K,J) + TEMP 
1 CONTINUE 
MATTMP(I1,J) = TEMP 
5 CONTINUE 
pd 8 I1=1,3 
DOgSin J alee 
TEMP = 0. 0DO 


pO7 kK =1,3 
TEMP = MATTMP(11,K) * TMATTR(K,J) + TEMP 


7 CONTINUE 
LIMAT(1I,1I1,J) = TEMP 
8 CONTINUE 
9 CONTINUE 
* DOUBLE PENDULUM INITIALIZATION 
TH2D0 = 0. 0DO 
TH3DO = 0.0DO 
TH30) "Pie. 
TH20 = PI/2.0 
L2 = 0.416D0 
L3 = 0.45100 
M2P = 30. 8D0 
M3P = 28. 6D0 
HF = 0.5D0 
DERIVATIVE 
NOSORT 
230 CALL ERRSET (208,256,-1,1,1) 
CALL UERSET( LEVELQ, LEVLDQ) 
x INITIALIZE MATRIX A AND B TO ZERO 
DO 240ene— ae? 
DO 225.0 = 1,27 
MATA(I,J) = 0.0DO 
235 CONTINUE 
MATB(I) = 0. 0DO 
240 CONTINUE 
% INPUT JOINT EQUATIONS 
* JOINT ZERO EQUATIONS 
x Wl X (Wl X RB/G1) 
ze RBG1(1) = JXO - BCGOGX(1) 
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* RBG1(2) = JYO - LCOGY(1) 
* RBG1(3) = JZO - LCOGZ(1) 
* VEG@AC ies W1(1) 
ze VECTA(2) = W1(2) 
* VECTA(3) = W1(3) 
a CALL CPROD(VECTAO,RBG1,MICO,MJCO ,MKCO) 
¥ VECTB0O(1) = MICO 
* VECTBO(2) = MJCO 
* VECTBO(3) = MKCO 
* CALL CPROD( VECTAO , VECTBO,MICO,MJCO ,MKCO) 
ie SINCE LINK ONE DOES NOT MOVE 
MgO = 0. ODO 
MJCO = 0. 0DO 
MKCO = 0. ODO 
a INPUT JOINT EQUATIONS 
* JOINT ZERO EQUATIONS 
X* W1 X (W1 X RB/G1) 
RBG1(1) = JXO - LCOGX(1) 
RBG1(2) = JYO - LCOGY(1) 
RBG1(3) = JZO - LCOGZ(1) 
VECTA(1) = W1(1) 
VECTA(2) = W1(2) 
VECTA(3) = W1(3) 
CALL CPROD(VECTA ,RBG1,MICO,MJCO,MKCO) 
VECTB(1) = MICO 
VECTB(2) = MJCO 
VECTB(3) = MKCO 
CALL CPROD( VECTA, VECTB,MICO,MJCO,MKCO) 
% W1 X (W1 X RA/G1) 


RAGI(1) = JX1, - LCOGX(1) 
RAGI1(2) = JY1 - LCOGY(1) 
Peis) = ele LCOGZ( 1) 
VECTA(1) = W1(1) 
VODAC2) = WiG@2) 
VECTA(3) = W1(3) 
CALL CPROD (VECTA,RAG1,MIC1,MJC1,MKC1) 
VECTB(1) = MIC1 
VECTB(2) = MJC1 
VECTB(3) = MKC1 
CALL CPROD (VECTA,VECTB,MIC1,MJC1,MKC1) 
x W2 X (W2 X RB/G2) 
RBG2(1) = JX1 - LCOGX(2) 
RBG2(2) = JY1 - LCOGY(2) 


RBG2(3) = JZ1 - LCOGZ(2) 
VECTA(1) = W2(1) 
VECTA(2) = W2(2) 
VECTA(3) = W2(3) 
CALL CPROD (VECTA,RBG2,MIC2,MJC2,MKC2) 
VECTB(1) = MIC2 
VECTB(2) = MJC2 
VECTB(3) = MKC2 
CALL CPROD (VECTA,VECTB ,MIC2,MJC2,MKC2) 
ve W2 X (W2 X RA/G2) 
he2( 1) = Je - LCOGX(@2) 
RAG2(2) = JY2 - LCOGY(2) 
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RAG2(3) = 


JZ2 - LCOGZ(2) 


VECTAC1) = W2(1) 
VECTA(2) = W2(2) 
VECTAC3) = W2(3) 
CALL CPROD (VECTA,RAG2 ,MIC3 ,MJC3 ,MKC3) 
VECTB(1) = MIC3 
VECTB(2) = MJC3 
VECTB(3) = MKC3 


CALL CPROD( VECTA, VECTB ,MIC3 ,MJC3 ,MKC3 ) 
W3 X (W3 X RB/G3) 
RBG3(1) = JX2 - LCOGX(3) 


RBG3(2) = JY2 - LCOGY(3) 
RBG3(3) = JZ2 - LCOGZ(3) 
VECTA(1) = W3(1) 
VECTA(2) = W3(2) 
VECTA(3) = W3(3) 
CALL CPROD (VECTA,RBG3,MIC4,MJC4,MKC4) 
VECTB(1) = MIC4 
VECTB(2) = MJC4 
VECTB(3) = MKC4 


CALL CPROD (VECTA,VECTE ,MIC4,MJC4,MKC4) 


INERTIA TRANSFORMATION 


DO 390 I = 2, 3 

TMAT(2,1) = -DCOS (_THZ ) 
TMAT(2,2) = -DSIN ( THZ ) 
TMAT(2,3) = 0. ODO 
THAT(3,1) = DRESKGD 
TMAT(3,2) = DRCSY(I) 
TMAT(3,3) = DRCSZ(I) 
VECTA(1) = TMAT(2,1) 
VECTAC2 0 = Trim 2) 2 
VECRACS) = Diem@? 3) 
VECTB(1) = TMAT(3,1) 
VECTB(2) = Tbem@s,2 
VECTB(3) = TMAT(3,3) 
CALL CPROD (VECTA,VECTB,MI1,MJ1,MK1) 
TMAT(1,1) = MI1 

THAT( 1,2) = Mom 

TMAT(1,3) = MK1 

TMATTR( 1,1) = TMAT(1,1) 
TMATTR(1,2) = TMAT(2,1) 
TMATIR(1,3) = THATC3,1) 
TMATTR(2,1) = TMAT(1,2) 
TMATTR(2,2) = TMAT(2,2) 
TMATTR( 2,3) = TMAT(3,2 
TMATTR(3,1) = TMA 1,3) 
TMATTR(3,2) = TMAT(2,3) 
- TMATTR(3,3) = TMAT(3,3) 
DO 375 11 = 1,3 

DO 375.) 

TEMP = 0. ODO 
DO 370 K =1,3 
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370 


Se) 


378 


380 


390 
sescvesede 


ses 
? 


ry 


4 


rhy 


295 


(5 


Ea ik eee s MATOIRK,J) + TEMP 


CONTINUE 
MATTMP(1I1,J) = TEMP 
CONTINUE 
DO 380 I: = 1,3 
DO 380 J =1,3 
TEMP = 0. ODO 


UGss/8 K = 1,3 
Pep e= MARIMPC II K)es TMATCK Ss) + TEMP 


CONTINUE 
NIMAT(11,J) = TEMP 
CONTINUE 
em 1) = NIMATCI, 1) 
IXYT(1) = NIMAT(1,2) 
IXZT(1) = NIMAT(1,3) 
IYYT(1) = NIMAT(2,2) 
IYZT(I) = NIMAT(2,3) 
IZZT(1) = NIMAT(3,3) 
CONTINUE 


ENTER CONSTANTS INTO MATRIX A *%%%% 


LINK ONE 
SUMSOR SBOKCES 
X DIRECTION 


MATA(1,1) = 1.0D0 
MATA(1,4) = Ml 
MATA(1,10) = -1. ODO 
MATB( 1) = 0.0D0 
Y DIRECTION 
MATA(2,2) = 1.0D0 
MATA(2,5) = Ml 
MATA(2,11) = -1.0D0 
MATB( 2) = 0.0D0 
Z, DIRECTION 
MATA(3,3) = 1.0D0 
MATA(3,6) = Ml 
MATA(3,12) = -1. 0DO 
MATB( 3) = -WGl 


EQUATIONS AT JOINT ZERO 
X DIRECTION 


MATA(4,4) = 1.0D0 
MATA(4,8) = RBG1(3) 
MATA(4,9) = -RBG1(2) 
MATB(4) = AXO - MICO 
Y DIRECTION 
MATA(5,5) = 1. 0D0 
MATA(5,7) = -RBG1(3) 
MATA(S,9) = RBG1(1) 
MATB(5) = AYO - MJCO 
Z, DIRECTION 
MATA(6,6) = 1.0D0 
MATA(6,7) = RBG1(2) 
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se 


+ 


MATA(6,8) = -RBG1(1) 
MATB( 6) = AZO - MKCO 
SUM OF MOMENTS EQUATIONS 
X DIRECTION 
MATA(7,2) = RBG1(3) 
MATA( 7,3) = -RBG1(2) 
MATAC 7,7) = -IXXT(1) 
MATA( 7.8) =) bai 1) 
MATAC 7,9) = IxXZT(1) 
MATAC 7,11) = -RAG1(3) 
MATAC 7,12) = RAG1(2) 
MATB( 7) = T1X - TOX 
Y DIRECRION 
MATA(8,1) = -RBG1(3) 
MATA(8,3) = RBG1(1) 
MATA(8,7) = IXYTCi) 
MATA(8,8) ‘= -IYYT(1) 
MATA(8,9) = IYZT(1) 
MATA(8,10) = RAG1(3) 
MATA(8,12) = -RAGI1(1) 
MATBC 8} = T1Y - TOY 
2 DERECTFON 
MATAC9,1) = RBG1(2) 
MATAC9,2) = -RBG1(1) 
MATAC9,7) = IUs2Z7G@ 
MATAC9,8) = JEeZiGly) 
MATA(9,9) = -IZZT(1) 
MATA( 9,10) = -RAG1(2) 
MATA(9,11) = RAG1(1) 
MATB(9) = 7T1Z - TOZz 
LINK TWO 
SUM OF FORCES 
X DIRECTION 
MATAC 10,10) = 1.0D0 
MATAC 10,13) = M2 
MATA(C 10,19) = -1. ODO 
MATB( 10) = 0.0D0 
Y DIRECTION 
MATAC 11,11) = 1. 0D0 
MATA(C 11,14) = M2 
MATAC 11,20) = -1. 0D0 
MATB( 11) = 0. O0DO 
Z DIRECTION 
MATA( 12,12) = 1.0D0 
MATAC 12,15) = M2 
MATA( 12,21) = -1.0D0 
MAMB@GI2) = -WG2 
EQUATIONS AT JOINT ONE 
X DIRECTION 
MATA(C 13,4) = -1.0D0 
MATA( 13,8) = -RAG1(3) 
MATA( 13,9) = RAGI(2) 
MATA(C 13,13) = 1. 0D0 
MATAC 13,17) = RBG2(3) 
MATAC 13,18) = -RBG2(2) 


ve 


we 


>) 


ve 


eve 


~¢ 
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MATB( 13) = je il = wakes 
Y DIRECTION 
MATA(14,5) = -1.0D0 
MATA(14,7) = RAGI(3) 
MATA(14,9) = -RAG1(1) 
MATA(14,14) = 1.0D0 
MATA(14,16) = -RBG2(3) 
MATA(14,18) = RBG2(1) 
MATB( 14) = Shue = MIC2 
Z DIRECTION 
MATA(15,6) = -1.0D0 
MATA(15,7) = -RAG1(2) 
MATA(15,8) = RAG1(1) 
MATA(15,15) = 1.0D0 
MATA( 15,16) = RBG2(2) 
MATA(15,17) = -RBG2(1) 
MATB( 15) = MKC1 - MKC2 
SUM OF MOMENTS EQUATIONS 
X DIRECTION 
MAmGiG. 11) = RBE2C3) 
MATA( 16,12) = -RBG2(2) 
MATA( 16,16) = -IXXT(2) 
Mem@aclo.17) =  IXYI(2) 
MATA( 16,18) = IXZT(2) 
MATA( 16,20) = -RAG2(3) 
MATA( 16,21) = RAG2(2) 
MATB( 16) Sie TIX 
Y DIRECTION 
MATA( 17,10) = -RBG2(3) 
MAmemiy 12) = RBG2(1) 
MATA(17,16) = IXYT(2) 
MATA(17,17) = -IYYT(2) 
MAmAGt7 16) = I¥YZ7(2) 
MATA( 17,19) = RAG2(3) 
MATA( 17,21) = -RAG2(1) 
MATB(17) = 12y my 
Z DIRECTION 
MATA(18,10) = RBG2(2) 
MATA( 18,11) = -RBG2(1) 
MATA(18,16) = IXZT(2) 
MATA(18,17) = IYZT(2) 
MATA(18,18) = -IZZT(2) 
MATA( 18,19) = -RAG2(2) 
MATA(18,20) = RAG2(1) 
MATB( 18) = Ny = We 
LINK THREE 
SUM OF FORCES 
X DIRECTION 
MATA(19,19) = 1.0D0 
MATA(19,22) = M3 
MATB( 19) = 0.0D0 
Y DIRECTION 
MATA(20,20) = 1.0DO0 
MATA(20,23) = M3 


iS 


s 
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MATB( 20 ) = 0. 0DO0O 
Z DIRECTION 

MATA( 21,21) = 1. 0DO 

MATA( 21,24) = M3 


MATB(21) = -WG3 
EQUATIONS AT JOINT TWO 
X DIRECTION 


MATA( 22,13) = -1.0D0 

MATA( 22,17) = -RAG2(3) 
MATA( 22,18) = RAG2(2) 
MATA( 22,22) = 1.0D0 

MATA( 22,26) = RBG3(3) 
MATA( 22,27) = -RBG3(2) 
MATB( 22) = MIC3 - MIC4 

Y DIRECTION 

MATA( 23,14) = -1.0DO 
MATA(23,16) = RAG2(3) 
MATA( 23,18) = -RAG2(1) 
MATA( 23,23) = 1.0DO0 

MATA( 23,25) = -RBG3(3) 
MATA( 23,27) = RBG3(1) 
MATB( 23) = MJC3 - MJC4 

Z DIRECTION 

MATA( 24,15) = -1.0DO0 

MATA( 24,16) = -RAG2(2) 
MATA(24,17) = RAG2C 1) 
MATA( 24,24) = 1.0D0 

MATA( 24,25) = RBG3(2) 
MATA( 24,26) = -RBG3(1) 
MATB( 24) = MKC3 - MKC4 


SUM OF MOMENTS EQUATIONS 
X DIRECTION 


FIRST LINK IS CONSTRAINED TO ROTATE IN Z@ DIRECTION 


MATA(25,20) = RBG3(3) 
MATA( 25,21) = -RBG3(2) 
MATA( 25,25) = -IXXT(3) 
MATA( 25,26) = IXYT(3) 
MATA( 25,27) = IXZT(3) 
MATB( 25) =e 
Y DIRECTION 
MATA( 26,19) = -RBG3(3) 
MATA(26,21) = RBG3(1) 
MATA( 26,25) = IXYT(3) 
MATA( 26,26) = -IYYT(3) 
MATA( 26,27) = IY2ZT(3) 
MATB( 26) = =n 
Z DIRECTION 

MATA( 27,19) = RBG3(2) 
MATA(27,20) = -RBG3(1) 
MATA(27,25) = IXZT(3) 
MATAC27.26) = | ines.) 
MATA( 27,27) = -IZ2ZT(3) 
MATB( 27) = -T27 

DO 600 I = 4,8 

DO 595 J = 1,27 


devesveceve 
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ris 


DD 


600 


605 


610 


* 
620 


624 


627 


630 


631 


S53 
635 


weulento 
ee #% 


seveve 


ve 


640 


v 


MATA(I,J) = 0. 0DO 
MATA(J,I) = 0.0DO 
CONTINUE 
MATB(I) = 0. ODO 
CONTINUE 
MATA(4,4) = 1. 0D0 
MATA(S,5) = 1. 0DO 
MATA(6,6) = 1. 0D0 
MATA(7,7) = 1. 0DO 
MATA(8,8) = 1. 0D0 
nomenon = 197 
MATA(18,J) = 0. 0DO 
MATA(27,J) = 0. 0DO 
CONTINUE 
MATA(9,9) = -(IZZT(1)+IZZT(2)+1IZ2ZT(3)) 
MATA(18,9) = -1.0DO 
MATA(18,18) = 1.0DO0 
MATB( 18) = 0.0D0 
MATA(27,9) = -1.0DO 
MATA(27,27) = 1.0DO0 
MATB( 27) = 0. 0D0 


CALL EQUATION SOLVER PROGRAM FROM IMSL 
CALL LEQT2F(MATA,M,N,IA,MATB, IDGT ,WKAREA, IER) 
IF (IER. EQ.0) THEN 
GOTO 640 
ELSE 
WRITE (*,624) IER 
FORMAT (15) 
Demeeo. I = 1.927 
WRITE (*,627) I 
FORMAT (17) 
NO ACWIE Cc ey) 3 
WRITE (*,630) J,MATA(I,J),J+1,MATA(I,J+1),J+2,MATA(I,J+2) 
FORMAT (15,F13.5,15,F13.5,15,F13. 5) 
CONTINUE 
WRITE (**,633) I,MATB(T) 
FORMAT (13,F15.5) 
CONTINUE 
CALL ENDJOB 
END IF 


FIND LCOGX,LCOGY ,LCOGZ,THETA VALUES ,WX,WY ,WZ 
MOUTPrED BY K.M. VERBOS 


JOINT ZERO 
FXO = MATB(1) 
FYO = MATB(2) 
FZO = MATB(3) 


LINK ONE 
SINCE LINK1 IS CONSTRAIN NOT TO MOVE ALL ACC AND VEL ARE ZERO 
AX1 = 0. 0D0 


alate ate 
er avy v8 


AY1 = 0.0DO 
AZ1 = 0. 0DO 
AX1 = MATB(4) 
VELX1 = JNTGRL(0. ,AX1) 
| OKOLED AE = INTGRL(X1,VELX1) 
LCOGK¢1) = BeeGxT 
AY1 = MATB(S) 
VELY1 = INTGRL(0O. ,AY1) 
LCOGY1 = INTGRL(Y1,VELY1) 
LGOGY(1) = LcoGy 
AZ1 = MATB(6) 
VELZ1 = INTGRL(0. ,AZ1) 
Veecz1 = INTGRL(Z1,VELZ1) 
PEOGzZ¢1) = Legent 
WD1X = MATB(7) 
W1X = INTGRL(0. ,WD1X) 
WDX(1) = WD1X 
W1(1) = W1X 
WD1Y = MATB(8) 
W1Y = INTGRL(0. ,WD1Y) 
WDY(1) = WD1Y 
W1(2) = W1Y 
WD1Z = MATB(9) 
W1Z = INTGRL(0. ,WD12Z) 
WDZ(1) = WD1Z 
W1(3) = Wile 


ADDED BY R.M. VERBOS 


THZ = INTGRL(O. ,W1Z) 
THZANG = THZ * RADEG 
COSTHZ = DCOS(THZ) 
SINTHZ = DSIN(THZ) 


IF THE 18T LINK IS CONSTRAIN TO ROTATE IN THE Z DIRECTION ONLY 
THE DIRECTION COSINE AND DIRECTION COSINE ANGLES ARE CONSTANT 
AND DO NOT NEED TO BE CALCULATED 


CALC DIRECTIONAL COSINES FOR LINK ONE 


DRCSX(1) = R@GGRA / LNCOGI 

DRCSY(1) = LCOGY1 / LNCOG1 

DRCSZ(1) = LCOGZ1 / LNCOG1 

AMP = DSQRT(DRCSX(1)*DRCSX(1)+DRCSY(1)*DRCSY(1)+... 
DRCSZ(1)**DRCSZ(1)) 

DRCSX(1) = DRCSX(1)/AMP 

DRCSY(1) = DRCSY(1)/AMP 

DRCSZ(1) = DRCSZ(1)/AMP 

DRCANX(1) = DACOS(DRCSX(1)) * RADEG 

DRCANY(1) = DACOS(DRCSY(1)) * RADEG 

DRCANZ(1) = DACOS(DRCSZ(1)) * RADEG 


JOINT LOCATION 


JX1 = LINKL1 * DRCSX(1) 
JY1 = LINKL1 * DRCSY(1) 
JZ1 = LINKL1 * DRCSZ(1) 
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* JOINT ONE 
735 FX1 = MATB(10) 
FY1 = MATB(11) 
FZ1 = MATB(12) 
rere LINK TWO 
740 AX2 = MATB(13) 
VELX2 = INTGRL(0. ,AX2) 
LCOGX2 § = INTGRL(X2,VELX2) 
LCOGX(2) = LCOGX2 
AY2 = MATB( 14) 
VELY2 = INTGRL(0. ,AY2) 
LCOGY2 = INTGRL(Y2,VELY2) 
LCOGY(2) = LCOGY2 
AZ2 = MATB(15) 
VELZ2 = INTGRL(0. ,AZ2) 
mege72) = INTGRE(Z2,VELZ2) 
LCOGZ(2) = LCOGZ2 
WD2X = MATB( 16) 
W2X = INTGRL(0. ,WD2X) 
WDX(2) = WD2x 
W2(1) = W2x 
WD2Y = MATB(17) 
W2Y = INTGRL(0. ,WD2Y) 
WDY(2) = WD2Y 
W2(2) = W2Y 
WD2Z = MATB(18) 
W2Z = INTGRL(0. ,WD2Z) 
WDZ(2) = DY 
W2(3) = W2Z 
* GET THE DIRECTION COSINES FOR THE LINK TWO 
DRCSX(2) = (LCOGK2 - JX1) / LNCOG2 
DRCSY(2) = (LCOGY2 - JY1) / LNCOG2 
DRCSZ(2) = (LCOGZ2 - JZ1) / LNCOG2 
790 AMP = DSQRT(DRCSX(2)*DRCSX( 2)+DRCSY(2)*DRCSY(2)+... 
DRCSZ(2)*DRCSZ(2)) 
DRCSX(2) = DRCSX(2)/AMP 
DRCSY(2) = DRCSY(2)/AMP 
DRCSZ(2) = DRCSZ(2)/AMP 
DRCANX(2) = DACOS(DRCSX(2)) * RADEG 
DRCANY(2) = DACOS(DRCSY(2)) * RADEG 
DRCANZ(2) = DACOS(DRCSZ(2)) * RADEG 
x JOINT LOCATION 
800 JX2 = JX1 + LINKL2 * DRCSX(2) 
JY2 = JY1 + LINKL2 * DRCSY(2) 
JZ2 = JZ1 + LINKL2 * DRCSZ(2) 
te JOINT TWO 
805 FX2 = MATB(19) 
FY2 = MATB(20) 
FZ2 = MATB(21) 
weve LINK THREE 
812 AX3 = MATB( 22) 
VELX3 = INTGRL(0. ,AX3) 
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LCOGX3 = INTGRL( X3,VELX3) 
LCOGX(3) = aeoGxs 

AY3 = MATB(23) 

VELY3 = INTGRL(O. , AY3) 
LCOGY3 = INTGRL(Y3,VELY3) 
LCOGY(3) = LCOGY3 

AZ3 = MATB( 24) 

VELZ3 = INTGRL(0. ,AZ3) 
MGOGZ3 = INTGRL(Z3,VELZ3) 
LCOGZ(3) = LCOGZ3 

WD3X = MATB(25) 

W3X = INTGRL(O. ,WD3X) 
WDX(3) = wex 

W3(1) = W3X 

WD3Y = MATB( 26) 

W3Y = INTGRL(0. ,WD3Y) 
WDY(3) = WD3Y 

W3(2) = W3Y 

WD3Z = MATB( 27) 

W3Z = INTGRL(0. ,WD3Z) 
WDZ(3) = WD3Z 

W3(3) = W3Z 


* CALC DIRECTIONAL COSINES FOR LINK THREE 


845 DRCSX(3) = (LCOGX3 - JX2) / LNCOG3 
DRCSY(3) = (LCOGY3 - J¥A—/ LNCOG3 
DRCSZ(3) = (LG0GZ3 - JZ2) /“NeeG3 
865 AMP = DSQRT(DRCSX(3)*DRCSX(3)+DRCSY(3)*DRCSY(3)+... 
DRCSZ(3)*DRCSZ(3)) 
DRCSX(3) = DRCSX(3)/AMP 
DRCSY(3) = DRCSY(3)/AMP 
DRCSZ(3) = DRCSZ(3)/AMP 
DRCANX(3) = DACOS(DRCSX(3)) * RADEG 
DRCANY(3) = DACOS(DRCSY(3)) * RADEG 
DRCANZ(3) = DACOS(DRCSZ(3)) * RADEG 
* TIP LOCATION 
"875 TIPX = JX2 + LINKL3 * DRCSX(3) 
** TIPY = JY2 + LINKL3 * DRCSY(3) 
ve TIPZ = JZ2 + LINKL3 * DRCSZ(3) 
a FIND THE ANGLE BETWEEN THE LIMKS 
880 ANG23X = DRCANX(2) - DRCANX(3) 


ANG23Y = DRCANY(2) - DRCANY(3) 
ANG23Z = DRCANZ(2) - DRCANZ(3) 
ANG23 = ANG23X + ANG23Y + ANG23Z 
* ANG12X = DRCANX(1) - DRCANX(2) 
* ANG12Y = DRCANY(1) - DRCANY(2) 
X* ANG12Z = DRCANZ(1) - DRCANZ(2) 
x DOUBLE PENDULUM CALCULATION 
PA = 1,3%*M3P*SIN( TH2-TH3)*TH3DOT*#*2 
PB = SIN(TH2)*G**(M3P4+M2P) 
PC = L3*M3P*COS(TH2-TH3) 
PD = L,2**(M3P+M2P) 
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ae = L2*SIN( TH2-TH3 )*TH2DOT**2 
Jee = SINC TH3)*G 

PG = COS(TH2-TH3)*L2 

PH = L3 

TH3DDT = (PE-PF+(( PG*PA+PG*PB)/PD))/CPH-(PG*PC/PD) ) 
THZDDT = (-PA - PB - PG*THSDDT)/PD 
TH3DOT = INTGRL(TH3D0,TH3DDT) 
TH2DOT = INTGRL(TH2D0 , TH2DDT) 

TH3 = INTGRL(TH30 , TH3DOT) 

THe = INTGRL( TH20 , TH2DOT) 
TH2ANG = TH2 * RADEG 


IF( TH2ANG. GT. 0. 0) THEN 
ANGTH2 = 180.0 - DRCANZ( 2) 
ELSE 
ANGTH2 = DRCANZ(2) - 180.0 

ENDIF 

TH3ANG = TH3 * RADEG 

IF( TH3ANG. GT. 0. 0) THEN 
ANGTH3 = 180.0 - DRCANZ(3) 


ELSE 
ANGTH3 = DRCANZ(3) - 180.0 
ENDIF 
END 
Sa0P 
FORTRAN 
* SUBROUTINE TO COMPUTE THE CROSS PRODUCT OF TWO VECTORS 


SUBROUTINE CPROD( VECTA, VECTB,MI,MJ,MK) 
940 IMPLICIT REAL*8 (A-Z) 
DIMENSION VECTA(3), VECTB(3) 


Ma = VECTA( 2) = VEeGpE( 3) @-eVECTAl 3).% VECTBC 2) 
MJ = VECTA(3) * VECTB(1) - VECTAC1) * VECTB(3) 
MK = VECTA( 1) * VECTB(2) - VECTAC2) * VECTB(1) 
RETURN 
END 
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APPENDIX C 


THREE DIMENSION VALIDATION PROGRAM 


TITLE NONSINGULAR 3-D VALIDATION PROGRAM 


Sevedededededicdedetotedetste de devote se teste Ks sede dete te deve Fee Ke tele sede RGEC K FECL Te TDC TEE TCTE TC He Se Te So eK Hee Wade ae ae ee ae cose eee 


* THIS IS A PROGRAM WHICH VALIDATES THE NON-SINGULARITY OF A THREE 

* LINK RIGID BODY MANIPULATOR WITHOUT THE USE OF THE STANDARD ROBOT 

* TRANSFORMATION MATRICES. THE ORIGINAL PROGRAM WAS WRITTEN BY LT 

* ATINOK AND HAS BEEN GREATLY MODIFIED TO INCLUDE THREE DIMENSIONAL 
*° MOTION AND GRAVITY BY LT ROBERT Mr virreoos. SEPTEMBER laa 


sviawasase’sa'sats uta = 2 ae ewewtsn nn’ s ose sets Sawa m ed = ewsn'sn'e ome wale ale atsa's als aton, eo! Oe ee we) oe) o atsaleatan'e wo! $ 
dededevededededesedede cede sede de deco ses veke ds vede deve cede Fe sede dese cess Te VENT GEG Seas a8 os 26 SOUR DE EUR AC SE IC TC Te Te TC TC eae cede 46 oo 7e ee 


ae 


TERMINAL 

METHOD ADAMS 

PRINT . O02, BRROR], ERRG@R2 (ERROKS., sANGIZ ano o 

CONTROL FINTIM =1.6, DELT = .0005, DELMAX = .1, DELPRT = .02 


D DIMENSION MATAC 27,27), MASS(3.,2) ,L03,2) ,RX(3,2),RY(3,2) R20 jee 

D DIMENSION IXX(3,2),742Z(03,2), 1X¥( 3, 2) g1YY( 3,2). 11203 2) 

D DIMENSION IMAT(3,3,3) ,NIMAT(3,3),TMAT(3,3), TMATIR(3,3) , Agi ee 
D DIMENSION LIMAT GS) 


a 28, IER, 1,J¢6,h,P.N, 2A, eT AL 11 

ARRAY MATB(27) ,MATC( 27) ,MATD( 27), LCOGX( 3S) GbeeGi ay EeeGz 
ARRAY VECTA(3) , VECTB(3) 

ARRAY WDX(3) ,WDY(3) ,WDZ(3) ,W1(3) .W2(3) ,W3(3) 

ARRAY RBG1(3),RAG1(3) ,RBG2(3) ,RAG2(3) ,RBG3(3) 

ARRAY WKAREA( 850) 

ARRAY IXXT(3), 1[YYTC3) ,IZZT(3) ay 13 ae Ss) See) 

ARRAY DRCANK(3) ,DRCANY(3) , DRCANZ(3) 

ARRAY DRCSXA(3) ,DRCSY(3) ,DRESZ(3), 


INITIAL 
Hiveteded INPUT Bnew se oe 
¥ INPUT PARAMETER CONSTANTS 
Do A = 15. 0D0 
Je = 0. 0DO0 
W = PI/2.0 
IDGT = 6 
G = 9.81D0 
N = 2/ ) 
M = 1 . 
IA = 27 
IER =0 
LEVELQ = 0 
LEVLDQ = 0 
¥ INPUT JOINT LOCATIONS IN METERS 
JXO = 0. 0D0 
JYO = 0. 0D0 
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x 


a 


20 


x 


vid 


Jz0) = 0,00 
JIxXi- = 0. ODO 
JY1 = 0. 0D0 
JZ1 = 0. 2D0 
JX2 = 0.0D0 
JY2 = 0.416D0 
jz2 = G@, 2D0 
INPUT DISTANCE FROM CENTER OF LINK TO CENTER OF MASS 
LCi) = 0, OSDO 
L(1,2) = 0. 15D0 
L( 23. 0. 208D0 
Con iO 08D0 
L(3,1) = 0. 2255D0 
L(3,2) = 0.2255D0 
INPUT THE LINK LENGTHS 
LINKL1 = 0. 20D0 
LINKL2 = 0.416D0 
LINKL3 = 0.451D0 
INPUT MASS AT LINK ENDS IN KILOGRAMS 
MASS(1,1) = 1. 0D0 
MASS(1,2) = 3. 0DO 
MASS(2,1) = 2. 2D0 
MASS(2,2) = 2.2D0 
MASS(3,1) = 8. 6DO 
MASS(3,2) = 8. 6DO 
INPUT LOCATION OF LINK CENTERS OF GRAVITY 
LCOGX(1) = 0.O0DO 
x1 = [COGX(1) 
LCOGY(1) = 0. 0DO 
Yl = LcOoGY(1) 
LCOGZ(1) = 0.10D0 
thik = [COGZ(1) 
LCOGX(2) = 0.0DO 
X2 = [LCOGX(2) 
LCOGY(2) = 0.208D0 
Y2 = Leese) 
LCOGZ(2) = 0. 20D0 
72 = [COGZ(2) 
neOGHe@sy = 0. 0D0 
X3 =) 16eEss) 
LCOGY(3) = 0.6415D0 
Y3 = coer 3) 
MeOGZ(S) = 0. 2D0 
Z3 = [C0GZ(3) 
INPUT MASS OF EACH LINK IN KG 
M1 = 4. 0D0 
M2 = 4. 4D0 
M3 = 17.2D0 
INPUT ACCELERATIONS OF JOINT ZERO 
AXO = 0.0DO 
AYO = 0. 0DO 
AZO = 0.0DO 
INPUT THE INITIAL DIRECTION COSINES 
DRCSX(1) = 0. 0DO 
DRCSY(1) = 0. 0DO 
DRCSZ(1) = 1. 0D0 
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DRCSXK(2) = 0. ODO 
DRCSY(2) = 1. 0D0 
DRCSZ(2) = 0. ODO 
DRCSK(3) = 0. ODO 
DRCSY(3) = 1. 0DO 
DRCSZ(3) = 0. ODO 
* INPUT THE INITIAL DIRECTION COSINE ANGLES 
DRCANX( 1) = 90. ODO 
DRCANY( 1) = 90. ODO 
DRCANZ(1) = 0O.ODO 
DRCANX(2) = 90. ODO 
DRCANY(2) = 0O.ODO 
DRCANZ(2) = 90. 0DO 
DRCANX(3) = 90. ODO 
DRCANY(3) = 0. ODO 
DRCANZ(3) = 90. ODO 
rye ork sr\ or) y INITIALIZE gel Sirs 
* OMEGA AND OMEGA DOT 
139 DO 146 I = 1,3 
W1CI) = 0. 0D0 
W2(I) = 0. 0D0 
W3(I) = 0. ODO 
WDXC I) = 0. ODO 
WDYC I) = 0. ODO 
WDZ( I) = 0700 
146 CONTINUE 
W1(3) = 2. 0D0 
W201) = 2. 0D0 
W3(C1) = 2. 0D0 
WDY(2) = 4. ODO 
WDY(3) = 4. ODO 
THZ = 0. 0D0 
* INITIALIZE MATRICES TO ZERO 


DONO. 1 — ey 
DO) 75 ee 
MATA(I,J) = 0. 0DO 


1/5 CONTINUE 
MATBC I) = 0. ODO 
MATC(I) = 0. ODO 
MATD( I) = 0. ODO 
180 CONTINUE 


ToasacerIe CALCULATIONS Jeveteted 
vs WEIGHTS (NEWTONS) 


eS) WG1 = M1*G 
WG2 = M2*G 
WG3 = M3*G 
* COMPUTE THE LENGTH FROM THE INBOARD JOINT TO COG 


LNCOG1 = DSQRT ( LCOGX(1)*LCOGX(1) + LCOGY(1)*LCOGY(1) +... 
LEOGZ( 1)*Leee 7G) =) 


[AZ = LCOGX(2) - Jae 
LY2Z = LCeer(2). = Ire 
LZ2 = LCOGZ(2) - JZ1 


LNCOG2 = DSQRT ( LX2*LX2 + LY2*LY2 + LZ2*LZ2 ) 


oh 
SH 


200 


ZOD 


225 


iage— LCGGACS) - JX2 
iiem— LGOey oS) JZ 
LZ23 =@&COGZ(3) - JZ2 


LNCOG3 = DSQRT ( 
COMPUTE FBD INERTIAS 
DO 225 I = 


LX3*LX3 + LY3*LY3 + LZ3*LZ3 ) 
ON POINT MASSES IN GLOBAL COORDINATES 


BASED 


ies 1 ) 
iol, 2) 
Ou 1) 
IYY(I,1) 
LYY(I,2) 
IYYT(I) 
mZ7(1. 1) 
W72z(1,2) 
R22 0( 1) 
ion. 1) 
ip 1 2) 
IXYT(I) 
171.1) 
CT?) 
IXZT(I) 
mz. 1) 
lez 2) 
IYZT(I) 


Pe GEA ( 


i Onel) 
ELSE 

ie chG 1) 
END IF 


(ieee, Cl) . LB. 


rel ) 
END IF 


ieotaei( 1) . LE. 


1 Z28( 1) 
ELSE 

Zar ) 
END IF 


IMAT(I,1,1) 
IMAT(I,1,2) 
IMAT(I,1,3) 
IMAT(1,2,1) 
mMAMCD 2,2) 
IMAT(I,2,3) 
IMAT(I,3,1) 
IMAT(I,3,2) 
IMAT(I,3,3) 


RX(I,1) 
RX(I,2) 
RY(I,1) 
RY(I,2) 
RZ(I,1) 
RZ(I,2) 


Pee 


SLC lop ale: 
Melos 
-L(I,1) 
ibe? 
=1(1,1 
ib 1 2 


sk 
sr 


as 


DRCSX(T) 
DRCSX(T) 
DRCSY(T1) 
DRCSY(T) 
DRCSZ(T) 
DRCS2Z(T) 


MASS(I,1) * ((RY(I,1) 
MASS(I,2) * ((RY(I,2) 
ix Gleio + TXX(1,2) 

MASS mea (1 , 1) 
MASS(I,2) * ((RX(I,2) 
iyyCWeny) + TYY(1,2) 

Mascmmeny © (CRXCT, 1) 
MASS(I,2) * ((RX(I,2) 


iy 
se 


\y 
x 


>) 
sk 


va 


RYVGL 1) 
RVG) 


Ri) 
ROC) 


RX(1I,1) 
RAC Iye2) 


77 eee) 
MASS(I,1) * RX(I,1) 
MASS(I,2) * RX(I,2) 
eerie) 4 PXY (1 2 
MASS(I,1) * RZ(I,1) 
MASS(I,2) * RZ(I,2) 
7G) Pe. 2)) 
MASS(I,1) * RY(I,1) 
MASS(I,2) * RY(I,2) 
mez T at) te 1YZC1.2) 
) . LE. .01) THEN 

.01 


hei bukit db tt ud bu i ad te a a ue a 


= iNOGrGn 


Os) aie 
= .0O1 


= IYYT(I1) 


-0O1) THEN 
= .01 


= [22ZTC(1) 


XG) 
IXYT(T) 
Ze) 
-IXYT(T) 
LYYT(T) 
IBeALC IS 
-1XZT(1) 
-IYZT(TI) 
IZ2Z2T(1) 


CONTINDE 
DUE TO LINK 1 CONSTRAINTS AND SYMMETRY LINK 1 INERTIAS ARE CONST 
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i Idle) 
Hs ROA GIL A) 


an 


* RX(1,8 
* RX(I,2) 


“a 


ORAL) 
eR AGE ) 


) 
) 
) 
) 
) 
) 


at 
ae 


at 


1 


(RZ(1,1) 
(RZ(1,2) 


(RZ(1,1) 
(RZ(1,2) 


(RY(I,1) 
C(RY(I,2) 


* 
¥¢ 


a 
* 


as 


s¢ 


IXXT(1) = IMAT(1,1,1) 
TMvned) = Dame} 
IXZ0C1). = TM sae. 3)) 
IYYT(1) = IMAT(1,2,2) 
IYZT(1) = IMAT(1,2,3) 
12211) = item so) 


* TRANSFORM THE GLOBAL INERTIAS TO LOCAL WHICH REMAIN CONSTANT 


DOg281 1° —"2 


PMAT(2,1) = =0DCOS (Guuncem 
TMAT(2,2) = -DSIN ( THZ ) 
TMAT(2,3) = 0. 0DO 
TMAT(3,1) = DR@ax(T) 
TMAT(3,2) = DRG@era1) 
TMAT(3,3) = DRCSZ(I) 
VECTA(1) = TMAT(2,1) 
WECTAC 2) = IMAgC 202) 
VECTA(3) = TMAT(2,3) 
VECTB(1) = TMAT(3,1) 
VECTB(2) = TMAT(3,2) 
VECTB(3) = TMAT(3,3) 
CALL CPROD (VECTA,VECTB,MI1,MJ1,MK1) 
THe (2,1) = cael 
TMAT(1,2) = MJ1 
TMAT(1,3) = MK1 
TMATTR(1,1) = TMAT(1,1) 
TMATTR(1,2) = TMAT(2,1) 
TMATTR(1,3) = TMAT(3,1) 
TMATTR(2,1) = TMAT(1,2) 
TMATTR(2,2) = TMAT(2,2) 
THATIR( 2), 3): seen Sen?) 
TMATTR(3,1) = TMAT(1,3) 
TMATTR(3,2) = TMAT(2,3) 
TMATTR(3,3) = TMAT(3,3) 
DO. 272 Tie aie 
DO 272) =a 
TEMP = 0. ODO 
DO 270 K =1,3 
TEMP = TMAT(I1,K) * IMAT(I,K,J) + TEMP 
270 CONTINUE 
MATTMP(11,J) = TEMP 
Pye CONTINUE 
we) 2 a3 
0) DEO ee ale 
TEMP = 0. ODO 
DO 278 K =1,3 
TEMP = MATTMP(11,K) * TMATTR(K,J) + TEMP 
Dae CONTINUE 
LIMAT(I,11,J) = TEMP 
280 CONTINUE 
281 CONTINUE 
DERIVATIVE 
NOSORT 
287 CALL ERRSET (208,256,-1,1,1) 
CALL UERSET( LEVELQ, LEVLDQ) 
z INITIALIZE MATRICES TO ZERO 
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WOr297° I = ele] 


DO 293 J = 1,27 
MATA(I,J) = 0. 0D0 
293 CONTINUE 
MATB(I) = 0.O0DO 
MATC(I) = 0.0DO 
MATD(I) = 0. 0DO 
297 CONTINUE 


rid 


x 
\s 


VALIDATION PORTI 
ENTER THE ANGULAR 
FOR THE CONSTRAIN 

THIL 
W1L 
WD1L 
TH1ANG 
WDX1G 
WDY1G 
WDZ1G 
WX1G 
WY1G 
W21G 
TH2L 
W2L 
WD2L 
TH2ANG 
WX2G 
WY2G 
W22G 
VECTA( 1) 
VECTA(2) 
VECTA(3) 
VECTB( 1) 
VECTB( 2) 
VECTB( 3) 
CALL CPROD 
WDX2G 
WDY2G 
WDZ2G 
TH3L 
W3L 
WD3L 
TH3ANG 
WX3G 
WY3G 
W23G 
WDX3G 
WDY3G 


dunn bid db db dd nb ot a ab i ad oe ol 


( 


riundku tnd bn dvb td be nb we neu 


ON OF PROGRAM ***%* 

AND LINEAR ACCELERATIONS, AND FORCES INTO MATRIX C 
Oiaeme, NEPTUNE Il 

Zavee LIME + PI/2.0 
2-00 

0. ODO 

TH1IL * RADEG 

CSGDO 

0. ODO 

WD1L 

0. ODO 

0. ODO 

W1L 

AMS) oe SNe al 

Zao 

0. ODO 

TH2L * RADEG 

W2L * SINC THIL) 

W2L * (- COSCTHIL) ) 
W1L 

0. ODO 

0. ODO 

WZ2G 

WA2G 

WY2G 

Crcpe 

VECTA, VECTB ,MI1,MJ1,MK1) 
MI1 
MJ1 
MK1 

0. ODO 
0. ODO 
0. ODO 
THe 
WX2G 
WY2G 
WZ2G 
WDX2G 
WDY2G 
WDZ2G 
LNCOG2 
LNCOG2 
LNCOG2 
LINKL2 
LINKL2 
LINKL2 
Red PEAS oor 


RADEG 


* 
ye 
rs 
X* 
ve 


se 


COS(TH2L) * COS(TH1L) 

COS(TH2L) * SIN(TH1L) 

SIN(TH2L) 

COS(TH2L) * COS(TH1L) 

COS(TH2L) * SIN(THIL) 

SIN( TH2L) 

LNCOG3 * COS(TH2L4TH3L) * COS(THIL) 
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rh 


ve 


Roy 
R3Z 
LTIPX 
Tiere 
LTIPZ 


CALCULATE THE 


vere 

AY1G 

AZiG 

VECTA( 1) 
VECTA(2) 
VECTA(3) 
VECTB(1) 
VECTB(2) 
VECTB(3) 
CALL CPROD 
VEGmaAGt) = 
VECTA( 2) 
VECTA(3) 
CALL CPROD 
VECTB( 1) 
VECTB(2) 
VECTB(3) 
CALL CPROD 
AX2G 

AY2G 

Azoe 

VECTA(1) 
VECTA( 2) 
VECTA(3) 
VECTB( 1) 
VECTB( 2) 
VECTB( 3) 
CALL CPROD 
VECTA( 1) 
VECTA(2) 
VECTA(3) 
CALL CPROD 
VECTB( 1) 
VECTB(2) 
VECTB(3) 
CALL CPROD 


WoW Ul 


Ol ut il 


SET MATRIX 
MATD( 27) 
MATD( 26) 
MATD( 25) 
MATD( 24) 
MATD( 23) 
MATD( 22) 
MATD( 21) 
MATD( 20) 
MATD( 19) 
MATD( 18) 


rnuu duu dt ud ue 


RJ2Y + LNCOG3 * COS(TH2L+TH3L) 
RJ2Z + LNCOG3 * SIN(TH2L+TH3L) 
RJ2X + LINKL3 * COS(TH2L+TH3L) 
RJ2Y + LINKL3 * COS(TH2L+TH3L) 
RJ2Z + LINKL3 * SIN(TH2L+TH3L) 

GLOBAL ACCELERATIONS 
0. ODO 
0. ODO 
0. ODO 
WDX2G 
WDY2G 
WDZ2G 
R2X 
R2Y 
R2Z 
(VECTA, VECTB,MI1,MJ1,MK1) 

WX 2G 

WY2G 

W22G 

(VECTA , VECTB ,MI2 ,MJ2 ,MK2) 
MI2 

MJ2 

MK2 

( VECTA, VECTB ,MI2,MJ2,MK2) 

MI1 + MI2 

MJ1 + MJ2 

MK1 + MK2 
WDX3G 
WDY3G 
WD23G 
R3X 
R3Y 
R3Z 
(VECTA , VECTB ,MI1,MJ1,MK1) 
WX3G 
WY3G 
WZ3G 
(VECTA, VECTB ,MI2,MJ2,MK2) 

MI2 
MJ2 
MK2 
(VECTA , VECTB ,MI2,MJ2,MK2) 

MI1 + MI2 

MJ1 + MJ2 

MK1 + MK2 


WDZ3G 

WDY3G 

WDX3G 

AZ3G 

AY3G 

AX3G 

=o" * AZ3G - Wes 
“Mot Awe G 

-M3 * AX3G 
WDZ2G 
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* SINC THIL) 


* COS(THIL) 
* SIN(TH1L) 
+ JZ1 


MATD(17) = WDY2G 
MATD(16) = WDX2G 
MATD(15) = AZ2G 
MATD(14) = AY2G 
MATD(13) = AX2G 
MATD(12) = MATD(21) - M2 * AZ2G - WG2 
MATD(11) = MATD(20) - M2 * AY2G 
MATD(10) = MATD(19) - M2 * AX2G 
MATD(9) = WDZ1G 
MATD(8) = WDY1G 
MATD(7) = WDX1G 
MATD(6) = AZ1G 
MATD(5) = AY1G 
MATD(4) = AX1G 
MATD(3) = MATD(12) - Ml * AZ1G - WG 
MATD(2) = MATD(11) - M1 * AY1G 
MATD(1) = MATD(10) - M1 * AX1G 
wee ~=DIRECT DYNAMICS PORTION OF PROGRAM *% 
ve INPUT JOINT EQUATIONS 
a JOINT ZERO EQUATIONS 
* W1 X (Wl X RB/G1) 
Reem 1) = JXO = LCOGX(1) 


RBG1(2) = JYO - LCOGY(1) 
RBG1(3) = JZO - LCOGZ(1) 
VECTA(1) = W1(1) 
VECTA(2) = W1(2) 
vem 3) = WiG3) 
CALL CPROD(VECTA,RBG1,MICO,MJCO,MKCO) 
VECTB(1) = MICO 
VECTB(2) = MJCO 
VECTB(3) = MKCO 
CALL CPROD( VECTA,VECTB ,MICO,MJCO,MKCO) 
ae W1 X (Wl X RA/G1) 
Poel!) = sxe LCOGX( 1) 
RAG1(2) = JY1 - LCOGY(1) 
RAG1(3) = JZ1 - LCOGZ(1) 
VECTA(1) = W1(1) 
VEGAIAC 2 emul? ) 
VECTA(3) = W1(3) 


CALL CPROD (VECTA,RAG1,MIC1,MJC1,MKC1) 
VEGIB( 1) = shine 1 
VECOB( 2) wRlent 
VECTB(3) MKC1 
CALL CPROD (VECTA,VECTB,MIC1,MJC1,MKC1) 
¥ W2 X (W2 X RB/G2) 
RBG2( 1) 


Jt - LCOOGX( 2) 


Fee2(2) = Yi = LCOGY(2) 
RBG2(3) = JZ1 - LCOG2(2) 
VECTA(1) = W2(1) 
VECTA(2) = W2(2) 
VECTA(3) = W2(3) 
CALL CPROD (VECTA,RBG2,MIC2,MJC2,MKC2) 
VECTB(1) = MIC2 
meme ( 2) = Muc2 
VECTB(3) = MKC2 
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-\= 


CALL CPROD (VECTA,VEGTS ,MiC2 HIG2Z ghey 
W2 X (W2 X RA/G2) 
RAG2(1) = JX2 - LCOGX(2) 
RAG2(2) = JY2 - LCOGY(2) 


RAG2(3) = JZ2 - LCOGZ(2) 
VECTA(1) = W2(1) 
VECTA(2) = W2(2) 
VECTA(3) = W2(3) 
CALL CPROD (VECTA,RAG2 ,MIC3,MJC3,MKC3) 
VECTB(1) = MIC3 
VECTB(2) = MJC3 
VECTB(3) = MKC3 


CALL CPROD( VECTA , VECTB ,NIC3 MICS iikGs> 
W3 X (W3 X RB/G3) 
RBG3(1) = JX2 - LCOGX(3) 
RBG3(2) = JY2 - LCOGY(3) 


RBG3(3) = JZ2 - LCOGZ(3) 
VECTA(1) = W3(1) 
VECTA(2) = W3(2) 
VECTA(3) = W3(3) 
CALL CPROD (VECTA,RBG3,MIC4,MJC4,MKC4) 
VECTB(1) = MIC4 
VECIBE2 a= MICS 
VECTB(3) = MKC4 


CALL CPROD (VECTA,VECTB,MIC4,MJC4 ,MKC4) 
INERTIA TRANSFORMATION 


TMAT(2,1) = -DG@S!( “THZ™ 
TMAT(2,2) = -DSIN ( THZ ) 
TMAT(2,3) = 0. 0D0 
DO 880 I = 2, 3 

TMAT(3,1) = DROSX(1) 
TMAT(3,2) = DRCSY(I) 
TMAT(3,3) = DRESZ(1) 
VECTA(1) = @MAT(2,1) 
VECTA(2) = TMAT(2,2) 
VECTA(3) = TMAT(2,3) 
VECTB(1) = TMAT(3,1) 
VECTB(2) = TMAT(3,2) 
VECTB(3) = TMAT(3,3) 
CALL CPROD (VECTA,VECTB,MI1,MJ1,MK1) 
TMAT(1,1) = Mal 
TMAT(1,2) = MJ1 
TMAT(1,3) = MK1 
TMATTR(1,1) = TMAT(1,1) 
TMATTR(1,2) = TMAT(2,1) 
TMATTR(1,3) = TMAT(3,1) 
TMATTR(2,1) = TMAT(1,2) 
TMATTR(2,2) = TMAT(2,2) 
TMATTR(2,3) = TMAT(3,2) 
TMATTR(3,1) = TMAT(1,3) 
TMATTR(3,2) = TMAT(2,3) 
TMATTR(3,3) = TMAT(3,3) 
mo 866 11 = ie 
DO 866 J = 1,3 

TEMP = 0. 0DO 

DO 864 K = 1,3 
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TEP Sree 1, K) * LIMATCI,K,J) + TEMP 


864 CONTINUE 
eine 11, Jye— TEMP 
866 CONTINUE 
DO 874 I1 = 1,3 
DO 874 J = 1,3 
TEMP = 0. ODO 


OO 872,.K = lee 
Teves Maleeie cil ,K) * TMATCK,J) + TEMP 


872 CONTINUE 
NIMAT(1I1,J) = TEMP 

874 CONTINUE 
IXXT(I) = NIMAT(1,1) 
IXYT(I) = NIMAT(1,2) 
IXZT(1) = NIMAT(1,3) 
IYYT(I) = NIMAT(2,2) 
IYZT(1) = NIMAT(2,3) 
IZZT(1) = NIMAT(3,3) 


880 CONTINUE 


* ENTER CONSTANS INTO MATRIX A 


ek LINK ONE 
* SUM OF FORCES 
a X DIRECTION 
887 MATA(1,1) = 1.0D0 
MATA(1,4) = M1 
MATA(1,10) = -1. ODO 
* Y DIRECTION 
MATA(2,2) = 1.0D0 
MATA(2,5) = Ml 
MATA(2,11) = -1.0D0 
oy Z DIRECTION 
MATA(3,3) = 1.0D0 
MATA(3,6) = Ml 
MATA(3,12) = -1.0D0 
zs EQUATIONS AT JOINT ZERO 
* X DIRECTION 
MATA(4,4) = 1.0DO0 
MATA(4,8) = RBG1(3) 
MATA(4,9) = -RBG1(2) 
x Y DIRECTION 
MATA(5,5) = 1.0D0 
MATA(5,7) = -RBG1(3) 
MATA(5,9) = RBG1(1) 
* Z DIRECTION 
MATA(6,6) = 1.0DO0 
MATA(6,7) = RBG1(2) 
MATA(6,8) = -RBG1(1) 
* SUM OF MOMENTS EQUATIONS 
zs X DIRECTION 
MATA(7,2) = RBG1(3) 
MATA(7,3) = -RBG1(2) 
MATA(7,7) = -IXXT(1) 
MATA(7,8) = IXYT(1) 
MATA(7,9) = IXZT(1) 
MATA(7,11) = -RAG1(3) 
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sisi 
a 


rhs 


oo9 


my 


se 


MATA(7,12) = RAG1(2) 
Y DIRECTION 
MATA(8,1) = -RBG1(3) 
MATA(8,3) = RBG1(1) 
MATA(8,7) = Uwecd 
MATA(8,8) = -IYYT(1) 
MATA(8,9) = IYZT(1) 
MATA(8,10) = RAG1(3) 
MATA(8,12) = -RAG1(1) 
Z DIRECTION 
MATA(9,1) = RBG1(2) 
MATA(9,2) = -RBG1(1) 
MATA(9,7) = IX2T(1) 
MATA(9,8) = IYZT(1) 
MATA(9,9) = -IZZT(1) 
MATA(9,10) = -RAG1(2) 
MATA(9,11) = RAG1(1) 
LINK TWO 
SUM OF FORCES 
X DIRECTION 
MATA( 10,10) = 1.0D0 
MATA(10,13) = M2 
MATA(10,19) = -1.0DO0 
Y DIRECTION 
MATA(11,11) = 1.0DO0 
MATA(11,14) = M2 
MATA( 11,20) = -1.0D0 
Z DIRECTION 
MATA( 12,12) = Seopo 
MATA(12,15) = M2 
MATA(12,21) = -1.0D0 
EQUATIONS AT JOINT ONE 
X DIRECTION 
MATA( 13,4) = -1.0DO0 
MATA( 13,8) = -RAG1(3) 
MATACI3 9) = —RSGqC2) 
MATA(13,13) = 1. 0D0 
MATA( 13,17) = RBG2(3) 
MATA( 13,18) = -RBG2(2) 
Y DIRECTION 
MATA(14,5) = -1.0D0 
MABEGA4 7) —=meReenCs) 
MATA(14,9) = -RAGI(1) 
MATA(14,14) = 1.0D0 
MATA( 14,16} = -RBG2(3) 
MATA(14,18) = RBG2(1) 
Z DIRECTION 
MATA(15,6) = -1.0D0 
MATA( 15,7) = -RAG1(2) 
MATA(15,8) = RAG1(1) 
MATA(15,15) = 1.0DO 
MATA( 15,16) = RBG2(2) 
MATA( 15,17) = -RBG2(1) 


SUM OF MOMENTS EQUATIONS 
X DIRECTION 
MATA(16,11) = 


RBG2(3) 
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ve 


¥e 


MATA( 16, 12) 
MATA( 16, 16) 
MATA( 16,17) 
MATA( 16,18) 
MATA( 16,20) 
MATA( 16,21) 
DIRECTION 

MATA( 17,10) 
MATA( 17,12) 
MATA( 17,16) 
MATA( 17,17) 
MATA( 17,18) 
MATA( 17,19) 
MATA( 17,21) 
DIRECTION 

MATA( 18,10) 
MATA( 18,11) 
MATA( 18,16) 
MATA( 18,17) 
MATA( 18, 18) 
MATA( 18,19) 
MATA( 18,20) 


LINK THREE 
SUM OF FORCES 


X 


EQUATIONS AT J 


X 


DIRECTION 
MATA( 19,19) 
MATA( 19,22) 
DIRECTION 
MATA( 20,20) 
MATA( 20,23) 
DIRECTION 
MATA( 21,21) 
MATA( 21,24) 


DIRECTION 

MATA( 22,13) 
MATA( 22,17) 
MATA( 22,18) 
MATA( 22,22) 
MATA( 22,26) 
MATA( 22,27) 
DIRECTION 

MATA( 23,14) 
MATA( 23,16) 
MATA( 23,18) 
MATA( 23,23) 
MATA( 23,25) 
MATA( 23, 27) 
DIRECTION 

MATA( 24,15) 
MATA( 24, 16) 
MATA( 24,17) 
MATA( 24,24) 
MATA( 24,25) 


-RBG2( 2) 
~IXXT(2) 
Para 2) 
PeZT( 2) 
-RAG2(3) 
RAG2( 2) 


-RBG2( 3) 
RBG2(1) 
IXYT(2) 

-IYYT(2) 
TY20 (2 
RAG2(3) 

-RAG2(1) 


RBG2(2) 
-RBG2(1) 
ieezir( 2) 
LYZT(2) 
-IZZT(2) 
-RAG2(2) 
RAG2(1) 


1. ODO 
M3 


1. ODO 
ge 


IL OIGIO, 
M3 


OINT TWO 


-1. ODO 
-RAG2(3) 
RAG2(2) 
1. ODO 
RBG3(3) 
-RBG3(2) 


-1. 0D0 
RAG2(3) 
-RAG2(1) 
1. ODO 
-RBG3(3) 
RBG3(1) 


-1. ODO 
-RAG2(2) 
RAG2(1) 
1. ODO 
RBG3(2) 


2) 


as 


rhs 


Sesesesese 


LOS 1 


1055 
1056 


1065 


MATA( 24,26) = 


“REGO 1) 


SUM OF MOMENTS EQUATIONS 


X DIRECTION 
MATA( 25,20) 
MATA( 25,21) 
MATA( 25,25) 
MATA( 25,26) 
MATA( 25,27) 

Y DIRECTION 
MATA( 26,19) 
MATA( 26,21) 
MATA( 26,25) 
MATA( 26, 26) 
MATA( 26,27) 

Z DIRECTION 
MATA( 27,19) 
MATA( 27,20) 
MATA( 27,25) 
MATA( 27,26) 
MATA( 27,27) 


RBG3( 3) 
=KBGo UZ) 
Sees ) 

IXYT(3) 

IXZT(3) 


ek BGsl >) 
RBG3( 1) 
ia 3) 
= Trerwi( 3 ) 
ir Ze ) 


RBG3( 2) 
-RBG3(1) 
IXZT(3) 
LYZT( 3) 
Sze) 


FIRST LINK IS CONSTRAINED TO ROTATE IN Z DIRECTION = *%%x%v 


DO 1056 I = 4, 


8 


Dond055 J = eed 
MATA(I,J) = 0.0DO 
MATA(J,I) = 0. 0DO 

CONTINUE 

CONTINUE 
MATA(4,4) = 1. 0D0 
MATA(S,5) = 1. 0DO0 
MATA(6,6) = 1. 0D0 
MATA(7,7) = 1. 0D0 
MATA(8,8) = 1. 0DO 

DO 1065 J = 1,27 

MATA(18,J) = 0.0DO 

MATA(27,J) = 0. 0DO 
CONTINUE 

1Z7mC2) = 

IZ2T(3) = 

MATA(9,9) = 

MATA(18,9) = -1.0D0 

MATA(18,18) = 1.0DO 

MATA(27,9) = -1.CDO 

MATA(27,27) = 1.0D0 


IZZT(2) + M2 * (DSQRT( LCOGX( 2)**2+LCOGY( 2) #2) )*72 
IZZT(3) + M3 * (DSQRT( LCOGX( 3 )%**2+LCOGY( 3)*%2) )**2 
-( 1Z2T( 1)+1Z2T(2)+12Z2T(3)) 


* MULTIPLY MATA * MATD TO OBTAIN TORQUES 


i> 


HS 


DO 75eede— 1.20 
SUM1 = 0. ODO 
BO 755°k = iy 


SUM1 = SUM1 + MATA(J,K) * MATD(K) 


CONTINUE 
MATC(J) = SUM1 
CONTINUE 
T2Z = -MATC(27) 
T2Y = -MATC(26) 
T2X = -MATC(25) 
Rize = T27e= “neers 
T1Y = T2Y - MATC(17) 


22 


v9 
BO? / 


Seeseveus 


LO? 


ea Z 


7 


1120 


WAS: 
1124 


Zo 
27 


TIX = T2X - MATC(16) 
TOZ = T1Z - MATC(9) 
Hovee= T1Y = MereCs 
TOX = T1X - MATC(7) 
ENTER KNOWNS INTO MATB 

MATB( 1) = 0.0D0 

MATB( 2) = 0.0D0 
MATB(3) = -WGl 

MATB(4) = AXO - MICO 
MATB(5) = AYO - MJCO 
MATB(6) = AZO - MKCO 
MATB(7) = T1X - TOX 
MATB(8) = T1Y - TOY 
MATB(9) =z = TOZ 
MATB(10) = 0.O0DO 
MATB(11) = 0.ODO 
MATB(12) = -WG2 
MATB(13) = MIC1 - MIC2 
MATB(14) = MJC1 - MJC2 
MATB(15) = MKC1 - MKC2 
MATB(16) = T2X - T1X 
MATB(17) = T2Y - T1Y 
MArB( 18) = T2Z - T1Z 
MATB(19) = 0.O0DO 
MATB(20) = 0.ODO 
MATB(21) = -WG3 
MATB(22) = MIC3 - MIC4 
MATB(23) = MJC3 - MJC4 
MATB(24) = MKC3 - MKC4 
Weams(25)) = =1o% 
MATB(26) = -T2Y 
MATB(27) = -T2Z 
FIRST LINK IS CONSTRAINED TO ROTATE IN Z DIRECTION 


DO 1107 I = 4,8 


MATB(I) = 0.0DO 
CONTINUE 

MATB(18) = 0.0DO 

MATB(27) = 0.0DO 


CALL EQUATION 


S 


OLVER PROGRAM FROM IMSL 
CALL LEQT2F(MATA,M,N,IA,MATB, IDGT,WKAREA, IER) 


Perak. EO.O)e HEN 
Goro 1133 


ELSE 


WRITE (*,1117) IER 


FORMAT (15) 


Ibe 1127 IT=1, 2/7 
Wire (*, 1120) 1 
FORMAT (17) 

DET1124 J=1, 27, 3 


WRITE (*,1123) J,MATA(1,J),J+1,MATACI,J+1) ,J+2,MATA(I,J+2) 
FORMAT (15,F13.5,15,F13.5,15,F13.5) 

CONTINUE 

WRITE (*,1126) I,MATB(1) 

FORMAT (13,F15.5) 

CONTINUE 


23 


scyese 


hss) 


1166 


Sexe 


megs) 


CALL ENDJOB 
END IF 
FIND LCOGX, LCOGY,LCOGZ,THETA VALUES ,WX,WY ,WZ 
JOINT ZERO 
FXO = MATE(1) 
FYO = MATB(2) 
FZ0 = MATBC3) 


LINK ONE 
SINCE LINK1 IS CONSTRAIN TO ROTATE AROUND THE Z ONLY 
AX1 = 0. ODO 
AY1 = 0. ODO 
AZ1 = 0. 0DO 
AX1 = MATB(4) 
VELX1 = INTGRE(O. ,AX1) 
Reece = INTGRL(X1,VELX1) 
LOOGKC1) = Leee 
AY1 = MATB(5) 
VELY1 = INTGRL(0. ,AY1) 
meoGul = INIGRL(Y1,VELY1) 
LCOGY(1) = LCOGY1 
AZ1 = MATB(6) 
VELZ1 = INTGRL(0. ,AZ1) 
LCOGZ1 = INTGRL(Z1,VELZ1) 
LCOGZ(1) = LeGGzi 
WD1X = MATB(7) 
W1X = INTGRL(0O. ,WD1X) 
WDX(1) = WD1X 
W1(1) = W1X 
WD1Y = MATB(8) 
W1Y = INTGRL(0. ,WD1Y) 
WDY(1) = WD1Y 
W1(2) = W1Y 
WD1Z = MATB(9) 
W1Z = INTGRL(2. ,WD1Z) 
WDZ(1) = WD1Z 
Ww1(3) = W1Z 
THZ = NTERL( 0. zZ) 
THZANG = THZ * RADEG 
COSTHZ = DCOS(THZ) 
SINTHZ = DSIN(THZ) 


IF THE 1ST LINK IS CONSTRAIN TO ROTATE IN THE Z DIRECTION ONLY 
THE DIRECTION COSINE AND DIRECTION COSINE ANGLES ARE CONSTANT 
AND DO NOT NEED TO BE CALCULATED 


JOINT ONE 
FX1 = MATB(10) 
FY1 = MATB(11) 
FZ1 = MATB(12) 

LINK TWO 
AX2 = MATB(13) 
VELX2 = INTGRL(-0. 416,AX2) 
LCOGX2 $= INTGRL(X2,VELX2) 
LCOGX(2) = LCOGX2 
AY2 = MATB(14) 
VELY2 = INTGRL(0. ,AY2) 
LCOGY2 = INTGRL(Y2,VELY2) 
LCOGY(2) = LCOGY2 
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ohy 


aC 
2 15 


zis 


22.0 


nlanto 
® 


1224 


AZ2 
VELZ2 
eee 
MeGez@2) 
WD2X 
W2X 
WDX(2) 
W2(1) 
WD2Y 
W2Y 
WDY(2) 
W2(2) 
WD2Z 
W22 
W2(3) 
WDZ(2) 


MATB( 15) 
INTGRL( 0. 416,AZ2) 
INTGRL( 22 , VELZ2) 
Leoawz 

MATB( 16) 
INTGRL( 2. ,WD2X) 
WD2X 

W2X 

MATB(17) 
INTGRL(0. ,WD2Y) 
WD2Y 

W2Y 

MATB( 18) 
INTGRL( 2. ,WD2Z) 
W2Z 

WD2Z 


GET THE DIRECTION COSINES FOR THE LINK TWO 


DRCSX(2) 
DRCSY(2) 
DRCSZ(2) 


AMP = DSQRT(DRCSX(2)*DRCSX(2)+DRCSY(2)*DRCSY(2)+... 


DRCSX(2) 
DRCSY(2) 
DRCSZ(2) 
DRCANX(2) 
DRCANY( 2) 
DRCANZ( 2) 


JOINT LOCATION 


(LCOGX2 - JX1) / LNCOG2 
(LCOGY2 - JY1) / LNCOG2 
(ReOG22 -) JZi MENCOG2 


DRCSZ( 2 )*DRCSZ( 2) ) 


DRCSK(2) /AMP 
DRCSY( 2) /AMP 
DRCSZ( 2) /AMP 


DACOS(DRCSX(2)) * RADEG 
DACOS(DRCSY(2)) * RADEG 
DACOS(DRCSZ(2)) * RADEG 


JX2 = JX1 + LINKL2 * DRCSX(2) 
JY¥2 = JY1 + LINKL2 * DRCSY(2) 
JZ2 = JZ1 + LINKL2 * DRCSZ(2) 

JOINT TWO 
FX2 = MATB(19) 
FY2 = MATB(20) 
FZ2 = MATB(21) 

LINK THREE 

AX3 = MATB(22) 
VELX3 = INTGRL( -1. 282,AX3) 
rCOcKe = INTGRL(X3, VELX3) 
LCOGX(3) = LCOGX3 
AY3 = MATB( 23) 
VELY3 = INTGRL(0. ,AY3) 
LCOGY3 = INTGRL(Y3, VELY3) 
LCOGY(3) = LCOGY3 
AZ3 = MATB( 24) 
VELZ3 = INTGRL(1. 282,AZ3) 
LCOGZ3 = INTGRL(Z3,VELZ3) 
Kaser¢s) = LCOGZ3 
WD3X = MATB(25) 
W3X = INTGRL(2. ,WD3X) 
WDX(3) = WD3X 
W3(1) = We 
WD3Y = MATB( 26) 
W3Y = INTGRL(0. ,WD3Y) 


DD 


WDY(3) = WD3Y 
W3(2) = W3Y 
WD32Z = MATB(27) 
W3Z = INTGRL(2. ,WD3Z) 
WDZ(3) = ez 
W3(3) =a37 
* CALC DIRECTIONAL COSINES FOR LINK THREE 
1249 DRCSX(3) = (LCOGX3 - JX2) / LNCOG3 
DRCSY(3) = (LCOGY3 - JY2) / LNCOG3 
DRCSZ(3) = (LCOGZ3 - Jz2) / LNCOG3 
AMP = DSQRT(DRCSX(3)*DRCSX(3)+DRCSY(3)*DRCSY(3)+... 
DRCSZ(3)*DRCSZ(3)) 
DRCSX(3) = DRCSX(3)/AMP 
DRCSY(3) = DRCSY(3)/AMP 
DRCSZ(3) = DRCS2Z(3)/AMP 
DRCANX(3) = DACOS(DRCSX(3)) * RADEG 
DRCANY(3) = DACOS(DRCSY(3)) * RADEG 
DRCANZ(3) = DACOS(DRCS2(3)) * RADEG 
* TIP LOCATION 
1261 TIPX = JX2 + LINKL3 * DRCSX(3) 


TIPY = JYO==eeL NAL oe DRES YS) 
TIPZ = JZ2 + LURES * DR@SzC3) 


a FIND THE ANGLE BETWEEN THE LIMKS 
1265 ANG23K = DRCANX(2) - DRCANX(3) 
ANG23Y = DRCANY(2) - DRCANY(3) 
ANG23Z = DRCANZ(2) - DRCANZ(3) 
ANG12X = DRCANX(1) - DRCANX(2) 
ANG12Y = DRCANY(1) - DRCANY(2) 
ANG12Z = DRCANZ(1) - DRCANZ(2) 
ANG12 = DRCANZ(2) - DRCANZ(1) 
ANG23 = ANG23X + ANG23Y + ANG23Z 
X CLACULATE THE ERROR BETWEEN KNOWN TIP POSITION AND CALCULATED 
ERROR1 = ABS ( LTIPX - TIPX ) / 0.867 * 100.0 
ERROR2 = ABS ( LTIPY - TIPY ) / 0.867 * 100.0 
ERROR3 = ABS ( LTIPZ - TIPZ ) / 0.867 * 100.0 
END 
STOP 
FORTRAN 
* SUBROUTINE TO COMPUTE THE CROSS PRODUCT OF TWO VECTORS 


SUBROUTINE CPROD(VECTA, VECTB,MI,MJ,MK) 
1284 IMPLICIT REAL*8 (A-Z) 
DIMENSION VECTA(3) , VECTB(3) 


MI = VECTA(2) * VECTB(C3) - VECTAC3) * VECTB(2) 
MJ = VECTAC3) * VECTBQ1) - VECIAGI)e*< VECTB( 3) 
MK = VECTA(1)_* VECTBC2Z) =\VEGine ss sVECTE GE) 
RETURN 
END 
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APPENDIX D 


ROBOT VALIDATION PROGRAM 


TITLE ROBOT TORQUE VALIDATION PROGRAM 


Sa ase Rr aid ide i ie ie a Oe Fak ee ie ee ie ae ee ie ee teed eee ee ee eee eee eke ee 


* THIS IS A PROGRAM THAT USES ACTUAL DATA FROM THE NEPTUNE II ROBOT TO * 
* OBTAIN TORQUES TO INPUT INTO THE NONSINGULAR SIMULATION OF A 3 LINK * 
* RIGID MANIPULATOR. THE ORIGINAL PROGRAM WAS WRITTEN BY LT ALTINOK * 


* AND HAS BEEN GREATLY MODIFIED TO INCLUDE 3 D MOTION AND GRAVITATIONAL* 
Seeeeerecls BY LY R. M. VERBOS USN SEPTEMBER 1988 a 
TSMC TS TSE TS FETE Te OSES TOTES TE TE TE TENE TE TE BS TEES TSDC TE TEEN TCT TS TE TE TE TOTES TE POVE LOBES EOE ETERS EEG PEER BEER REE RSC 
TERMINAL 


METHOD RKSFX 

mens . 10,DRCANZ(3) 

Moen FINTIM =7.0, DELT = .01, DELMAX = .1, DELPRT = .10 
SAVE .10, DRCANZ(3) 

GRAPH (DE=TEK618) TIME ,DRCANZ(3) 


D DIMENSION MATA( 27,27) ,MASS(3,2),L(3,2),RX(3,2),RY(3,2),R2Z(3,2) 

D DAMEN SIONM@IX(3,2),1X2(03,2),1XY(3,2),1YY(3,2),1YZ(3,2),12Z2(3,2) 

D DIMENSION IMAT(3,3,3),NIMAT(3,3),TMAT(3,3),TMATTR( 3,3) ,MATIMP( 3, 3) 
D DIMENSION LIMAT(3,3,3) 


FIXED IER,I,J,M,K,P,N,IA,IDGT,A,I1 

ARRAY MATB( 27) ,LCOGX(3), LCOGY(3) , LCOGZ(3) 

ARRAY VECTA0(3),VECTBO(3) , VECTA1(3) , VECTB1(3) , VECTA2( 3) , VECTB2(3) 
ARRAY VECTA(3) , VECTB(3) 

ARRAY WDX(3) ,WDY(3),WDZ(3),W1(3) ,W2(3) ,W3(3) 

ARRAY RBG1(3),RAG1(2),RBG2(3) ,RAG2(3),RBG3(3) 

ARRAY WKAREA( 850) 

Meee X<T( 3), 1YYT( 3) , 1ZZT( 3) »PXYTCS3) , IXZT(3) , IYZT(3) 

ARRAY DRCANX(3) ,DRCANY(3) ,DRCANZ(3) 

ARRAY DRCSX(3),DRCSY(3),DRCSZ(3) 


INITIAL 
Somer sere INPUT Serevestese 
* INPUT PARAMETER CONSTANTS 
IDGT = 6 
G = 9. 81D0 
N = 27 } 
M=1 : 
IA = 27 
IER = 0 
LEVELQ = 0 
LEVLDQ = 0 
* INPUT DISTANCE FROM COG OF LINK TO CENTER OF MASS 
* FOR EACH LINK ENDS 


7 


‘ 


INPUT DHE LINK 


11:0 


OS 
1210, 


L(1,1) = 0. 1953D0 
L(1,2) = 0.0651D0 
L(2,1) = O-2oens 
L(2,2) = 0.208D0 
L(3,1) = 0. 1746D0 
L(3,2) = 0. 1746D0 
LENGTHS OF THE ROBOT 
LINKL1 = 0. 2604D0 
LINKL2 = 0. 416D0 
LINKL3 = 0. 3498D0 
INPUT JOINT LOCATIONS IN METERS 
JXO = 0.0D0 
JYO = 0. 0D0 
3ZO = 0. 0D0 
JX1 = 0. 0D0 
JY1 = 0. 0D0 
JZ1 = 0. 2604D0 
IX2 = 0. 0De 
JY2 = 0.416D0 
JZ2 = 0. 2604D0 
INPUT TORQUE CONSTANTS 
TOX = 0.0D0 
TOY = 0.0D0 
TOZ = 0. 0D0 
TIX = 0.0D0 
T1Y = O80D0 
Tez = 0. ODO 
T2X = 0.0D0 
TOY. = "OmNo 
TOY = 0.0D0 
Te = 0. ODO 
Tee = 0. 0D0 
TIFNC = 0. ODO 
T2FNC = 0.0DO 
INPUT MASS AT LINK ENDS IN KILOGRAMS 
MASS(1,1) = 2.273D0 
MASS(1,2) = 6.818D0 
MASS(2,1) = 0.455D0 
MASS(2,2) = 0.455D0 
MASS(3,1) = 5.909D0 
MASS(3,2) = 5.909D0 
INPUT LOCATION OF LINK CENTERS OF GRAVITY 
LCOGN = Om OO 
X1 =" BeeG)) 
LCoGu¢1) =  Onomo 
Y1 = | Teseue) 
LCOGZ(1) =) omens 
Bi) = ECtena 
LCOGX(2) = OmoO 
1D = [66GK(2) 
LOOGY(2) = Us2ueDo 
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pe = LCOGY(2) 
LCOGZ(2) = 0. 2604D0 
L2 = LCOGZ(2) 
LCOGX(3) = QO. ODO 
X3 = LCOGK(3) 
LCOGY(3) = 0.4159D0 
¥3 = LCOGY(3) 
LCOGZ(3) = 0O.0858DO0 
La = LCOGZ(3) 
¥ INPUT MASS OF EACH LINK IN KG 
M1 = 9.091D0 
M2 = 0.910D0 
Mee" 11, 615D0 
* INPUT ACCELERATIONS OF JOINT ZERO 
AXO = 0. ODO 
AYO = 0. O0DO 
AZO = QO. ODO 
* INPUT THE INITIAL DIRECTION COSINES 
DRCSX(1) = OQ. ODO 
DRCSY(1) = 0. ODO 
DRCSZ(1) = 1. 0D0 
DRCSX(2) = 0. ODO 
DRCSY(2) = 1. ODO 
DRCSZ(2) = 0. 0DO 
DRCSX(3) = 0. ODO 
DRCSY(3) = 0. ODO 
DRCSZ(3) = -1.0D0 
* INPUT THE INITIAL DIRECTION COSINE ANGLES 
DRCANX( 1) = 90. ODO 
DRCANY( 1) = 90. ODO 
DRCANZ(1) = Q. ODO 
DRCANX(2) = 90. ODO 
DRCANY(2) = 0.0DO 
DRCANZ(2) = 90. ODO 
DRCANX(3) = 90. ODO 
DRCANY( 3) = 90. ODO 
DRCANZ(3) = 180. ODO 
rider isrierd TN Ala ZE Jevededeve 
ve OMEGA AND OMEGA DOT 
160 DO 170 Il=1,3 
W1( 1) = Q.QD0 
W2(1) = 0. ODO 
W3(T1) = 0. ODO 
WDX( I) = Q. ODO 
WDY(I) = 0. ODO 
WDZ( I) = 0. ODO 
L718, CONT INUE 
W3(1) = 0. 158D0 
W3IC = W3(1) 
THZ = 0. ODO 


a 


LS 


180 


wes'ese wtae! 
ees vd #% ay 


ve 


190 


7)0N0) 


INITIALIZE MATRIX A AND B TO ZERO 


DO 180 1 ser 


7 


DO 17570 w 27 


MATACI,J) = 0. ODO 


CONTINUE 
MATB( I) = 
CONTINUE 


CONSTANT TO CONVERT STRIP DATA TO DELTA P 


RDVTOV 
RAREA 
RLA 
RVTOP 
RLBTON 
RINTOM 
RCNFAC 


0. 200 


10,000 


CALCULATIONS 


WEIGHTS (NEWTONS ) 
WG1 = M1*G 
WG2 = M2*G 
WG3 = MN3*G 


COMPUTE THE LENGTH 


0. 00312D0 
0.0597/2.0 


4. 448D0 
59 23700 
RDVTOV*RVTOP*RAREA*RLA*RLBTON*RINTOM** 2 


0.0.0 


wawewe utente 
@% @€% #9 Fd ER 


FROM THE INBOARD JOINT TO COG 


LNCOG1 = DSQRT ( 


LX2 = LCOGX(2) 
LY2 = LCOGY(2) 
LZ2 = LCOGZ(2) 
LNCOG2 = DSORT 
LX3 = LE@enes) 
LY3 = LCOGY(3) 
LZ3 = LCOG2Z(3) 


LCOGX(1)*LCOGX( 1) + LCOGY(1)*LCOGY(1) +... 


LCOGZ( 1) LCGGZ ame) 


JX1 
Jal 


=e i 


LNCOG3 = DSQRT ( 
V3IC = LNCOG3 * W3(1) 


ci 29 


L(3,2) = (13.5 * RONFAC) / ( MASS(3,2).™ G) 


LX2 
2 
sirz 
JZ2 


*LX2 + LY2*LY2 + LZ2**LZ2 ) 


LX3*LX3 + LY3*LY3 + LZ3*LZ3 ) 


- LNCOG3 


COMPUTE INITIAL INERTIAS BASED ON POINT MASSES 
IN GLOBAL COORDINATES 


DOS 225 1 >= i 


DRCSX(I) 
DRCSX(I) 
DRCSY(I) 


* DRCSY(I) 


DRCSZ(I1) 
DRCSZ(1) 


MASS(I,1) * ((RY(I,1) * RY(I,1)) 
MASS(I,2) * (CRY(1,2) * RYC(I,2)) 


MASS(I,1) * (CRX(I,1) * RX(I,1)) 
MASS(1,2) * ((RX(1,2) * REC LOD 


RX(I,1) = -L(I,1) * 
RX(I,2) = LC(1,2) * 
RY(I,1) = -L(1,1) * 
RY(1,.2) = 9eer) * 
RZGl. 1) =seene1).* 
RZC1,2) = —ebe2 

IXX(I,1) = 

IXX(I,2) = 

IXXT(I) = IXX(I,1) + IXX(I,2) 

IYY(I,1) = 

IYY(I,2) = 

IYYT(I) = IYY(I,1) + IYY(I,2) 
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+ Geer) 
+ (Raf 2) 


+ (RZ(I,1) 
+ (RZ(I,2) 


¥ 
is 


ve 
m5 


RZ( Ty 
RZ( 1,2 


RZC 1a 
RZ( 2 


ee a NY NY 


VY WY wy NY 


VS / NW 


171.1) 
1221.2) 
TZ2T(T) 

eC 1,1) 
IXY(I,2) 
IXYT(I) 

eZ 1.1) 
TZ, 2) 
IXZT(I) 

iv Z( 1,1) 
m7 1,2) 
NAG 

Tie ( Txoan 
IXXT( 1) 
ELSE 

ier 1) 
END IF 


205 


Hel nnb dd bd bd ou eu 


EPC LYY TCL). 


IYYT( I) = 
ELSE 
iexyTCl) = 
END IF 


ie eka ae 


IZzT(I) = 
ELSE 
IzZzT( I) = 
BINDS 


MASS(I,1) * ((RX(I,1) 
MASS(I,2) * ((RX(I,2) 
arate ye 170 le) 
MASS(I,1) * RX(I,1) * 
MASS(I,2) * RX(I,2) * 
IXY(1I,1) + IXY(I,2) 
MASS(I,1) * RZ(I,1) * 
MASS(I,2) * RZ(I,2) * 
TAC ay) <2 OAC 
MASS(I,1) * RY(I,1) * 
MASS(I,2) * RY(I,2) * 
ieee 2 1,2) 
.LE. .10) THEN 
10 


= IXXT(I) 


Oda THEN 
i 


VC 1) 


OAs) s EEN 


i724 1) 


ero o@lee))) + (RiGee) * RY(1I;1))) 
ere ler) +t (Rieiee2) * 6 RY(CI;2))) 


RY(I,1) 
RY(I,2) 


RxC 1,1) 
oS Calls 2 


R2(1I,1) 
RZ(1,2) 


iiean¢ 1, Vet) 
iiaen( 112 ) 
IMAT(I,1,3) 
IMAT(I,2,1) 
IMAT(1,2,2) 
IMAT(I,2,3) 
IMAT(I,3,1) 
IMAT(1,3,2) 
IMAT(1,3,3) 
CONTINUE 


IXXT(I) 
IDGALCICS 
IXZT(1) 
-IXYT(I) 
IYYT(1I) 
IYZT(I) 
-1XZ1(1) 
-1¥Z7(1) 
ZAZE( 1) 


22D 


x DUE TO LINK 
iene) 
IXYT( 1) 
IXZT( 1) 
IYYT(1) 
IYZT(1) 
ALCS 


1 CONSTRAINTS LINK 1 INERTIAS ARE CONSTANT 
IMAT(1,1,1) 
IMAT(1,1,2) 
IMAT(1,1,3) 
Tame) 
IMAT( 1,2, 3) 
IMAT(1,3,3) 


* TRANSFORM THE INITIAL INERTIAS TO LOCAL COORDINATED 


DO 9 I = 2, 3 
TMAT(2,1) = -DCOS ( THZ ) 
TMAT(2,2) = -DSIN ( THZ ) 
TMAT(2,3) = 0.0DO0 
TMAT(3,1) = DRCSX(I) 
TMAT(3,2) = DRCSY(I) 
TMAT(3,3) = DRCSZ(1I) 
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VECTA(1) = TMAT(2,1) 
VECTA@2) = TMAG( 2.2) 
VECTA(3) = dmAtT(?. 3) 
VECTB(1) = TMAT(3,1) 
VECTB(2) = TMAT(3,2) 
VECTB@3) = DHAT(3, 3) 
CALL CPRCD (VECTA,VECTB,MI1,MJ1,MK1) 
TMAT(1)1) = MIi 
TMAT(1,2) = MJ1 
TMAT(1,3) = MK1 
TMATTR(1,1) = TMAT(1,1) 
TMATIER( 1,20 = Gam 2 1) 
TMATTR(1,3) = TMAT(3,1) 
TMATTR( 2,1) = TMAT(1,2) 
TMATTR( 2,2) = TMAT(2,2) 
TMATTR(2,3) = TMAT(3,2) 
TMATTR(3,1) = TMAT(1,3) 
TMATTR(3,2) = TMAT(2,3) 
TMATTR(3,3) = TMAT(3,3) 
OLS 1 iam 
DO Sa — "1 3 

TEMP = 0. ODO 

Do 1 ee =] es 

TEMP = TMAT(I1,K) * IMAT(1,K,J) + TEMP 
1 CONTINUE 
MATTMP(1I1,J) = TEMP 
5 CONTINUE 

pos I1 = 1,3 
pos J =1,3 

TEMP = 0.0D0 


D7 K =173 
TEMP = MATTMP(I1,K) * TMATTR(K,J) + TEMP 


7 CONTINUE 
LIMAT(I,I1,J) = TEMP 
8 CONTINUE 
9 CONTINUE 
DERIVATIVE 
NOSORT 
230 CALL ERRSET (208,256,-1,1,1) 


CALL UERSET( LEVELQ, LEVLDQ) 


x INITIALIZE MATRIX A AND B TO ZERO 
DO 240 I = 1,27 
DO 235 J = 1,27 
MATA(I,J) = 0. ODO 


235 CONTINUE 

MATB(I) = 0. 0DO 
240 CONTINUE 
x INPUT JOINT EQUATIONS 


a 


Ny 


aS. 


JOINT ZERO EQUATIONS 
Wl guess RB/G1) 
RBG1(1) = JXO - LCOGX(1) 
RBG1(2) = JYO - LCOGY(1) 
RBG1(3) = JZO - LCOGZ(1) 
VEGPA( 1) = W1(1) 
VECTA(2) = W1(2) 
VECTA(3) = W1(3) 
CALL CPROD(VECTAO ,RBG1 ,MICO,MJCO,MKCO) 
VECTBO(1) = MICO 
VECTBO(2) = MJCO 
VECTBO(3) = MKCO 
CALL CPROD( VECTAO, VECTBO ,MICO ,MJCO ,MKCO) 


INPUT JOINT EQUATIONS 
JOINT ZERO EQUATIONS 
Wl X (Wl X RB/G1) 


RBG1(?) = JXO - LCOGX(1) 
RBG1(Z2) = JYO - LCOGY(1) 
REGICS) @—=sZ0u= BOOCZCT) 
VECTA(1) = W1(1) 
VECTA(2) = W1(2) 
VECTA(3) = W1(3) 


CALL CPROD(VECTA,RBG1,MICO,MJCO ,MKCO) 
VECTB(1) = MICO 
VECTB(2) = MJCO 
VECTB(3) = MKCO 
CALL CPROD( VECTA,VECTB ,MICO,MJCO,MKCO) 
Wl X (Wl X RA/G1) 
RAGI1(1) = JX1 - LCOGX(1) 


RAG1(2) = JY1 - LCOGY(1) 
RAG1(3) = JZ1 - LCOGZ(1) 
VECTA( 1) W1(1) 


VECTA(2) = W1(2) 
VECTA(3) = W1(3) 

CALL CPROD (VECTA,RAG1,MIC1,MJC1,MKC1) 
VECTB(1) = MIC1 
VECTB(2) = MJC1 
VECTB(3) = MKC1 

CALL CPROD (VECTA,VECTB ,MIC1,MJC1,MKC1) 

W2 X (W2 X RB/G2) 

RBG2(1) 
RBG2(2) 
RBG2(3) 
VECTA( 1) 
VECTA(2) = W2(2) 
VECTA(3) = W2(3) 

CALL CPROD (VECTA,RBG2 ,MIC2,MJC2,MKC2) 
VECTB(1) = MIC2 
VECTB(2) = MJC2 
VECTB(3) = MKC2 

CALL CPROD (VECTA,VECTB ,MIC2 ,MJC2 ,MKC2) 

W2 X (W2 X RA/G2) 


UX ieee COGX( 2) 
Jee LecOGyY (CZ) 
JZ - LCOGZ(2) 
W2(1) 


RAGe( hy) = IX2.- LCOGK( 2) 
RG2(2) =wWY2 - LCOGY(2) 
RAG? (3) = MizZ2 - LCOGZ(2) 
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¥c 


37.0 


VECTA(1) = W2(1) 
VECTA(2) = woc 2) 
VECTA(3) = W2(3) 
CALL CPROD ( VECTA,RAG2,MIC3,MJC3,MKC3) 
VECTB(1) = MIC3 
VECTB(2) = MJC3 
VECTB(3) = MKC3 


CALL CPROD( VECTA,VECTB ,MIC3 ,MJC3,MKC3) 


VECTA( 1) 
VECTA( 2) 
VECTA( 3) 
CALL CPROD ( 
VECTB( 1) 
VECTB( 2) 
VECTB( 3) 
CALL CPROD (VECTA,VECTB ,MIC4,MJC4,MKC4) 


CONTINUE 


W3 X (W3 X RB/G3) 
RBG3( 1) 
RBs G2 ) 
RBG3(3) 


JX2 - LCOGX(3) 

JY¥2 - LCOGY(3) 

JZ2 - LCOGZ(3) 

W3(1) 

W3(2) 

W3(3) 
ECTA ,RBG3,MIC4,MJC4,MKC4) 
MIC4 

MIC4 
= MKC4 


iuiut< 


INERTIA TRANSFORMATION 


DO 390 I = 2, 3 

TMAT(2,1) = -DCOS ( THZ ) 
TMAT(2,2) = -DSIN ( THZ ) 
TMAT(2,3) = 0. 0DO 
TMAT(3,1) = DRCSX(I) 
TMAT(3,2) = DRCSY(T) 
TMAT(3,3: = DRCSZ(I) 
VECTA(1) = TMAT(2,1) 
VECTA(2) = TMAT(2,2) 
VECTA(3) = TMAT(2,3) 
VECTB(1) = TMAT(3,1) 
VECTB(2) = TMAT(3,2) 
VECTB(3) = TMAT(3,3) 
CALL CPROD (VECTA,VECTB,MI1,MJ1,MK1) 
TMAT(1,1) = MI1 
TMAT(1,2) = MJ1 
TMAT(1,3) = MK1 
TMATTRG1,1) = Timea) 
TMATTR(1,2) = TMAT(2,1) 
TMATTR(1,3) = TMAT(3,1) 
TMATTR(2,1) = TMAT(1,2) 
TMATTR(2,2) = TMAT(2,2) 
TMATTR( 2,3) = TMAT(3,2) 
TMATTR( 3,1) = TMAT(1,3) 
TMATTR(3,2) = TMAT(2,3) 
TMATTR(3,3) = TMAT(3,3) 
DOMeee le 1.3 
DOs75nJ == tee 

TEMP = 0. ODO 

mer v0) 1k Sal 2 

TEMP = TMATTR(11,K) * LIMAT(I,K,J) + TEMP 
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378 


380 


x 


MALTMNP@E1 ,@) = TEMP 


CONTINUE 

WOrscO I] = 158 

DO 330 J = 1,3 
TEMP = 0. ODO 


DO 378 K =1,3 
TEMP = MATIMP(I1,K) * TMAT(K,J) + TEMP 


CONTINUE 
NIMAT(1I1,J) = TEMP 
CONTINUE 
ieecic1) = NIMAm( 141) 
ivr) = NIMAT@, 2) 
IXZT(I) = NIMAT(1,3) 
IYYT(1) = NIMAT(2,2) 
IYZT(1) = NIMAT(2,3) 
IZZT(1) = NIMAT(3,3) 
CONTINUE 


ENTER CONSTANTS INTO MATRIX A *vvevevs 


LINK ONE 
SUM OF FORCES 
X DIRECTION 


MATA(1,1) = 1.0D0 
MATA(1,4) = Ml 
MATA( 1,10) = ~1.0DO 
MATB( 1) = 0.0D0 
Y DIRECTION 
eae?) = 1, 6D0 
MATA(2,5) = Ml 
MATA(2,11) = ~1.0D0 
MATB( 2) = 0.0D0 
Z DIRECTION 
MATA(3,3) = 1.0D0 
MATA(3,6) = M1 
MATA(3,12) = -1. ODO 
MATB(3) = -WGl 


EQUATIONS AT JOINT ZERO 
X DIRECTION 


MATA(4,4) = 1.0D0 
MATA(4,8) = RBG1(3) 
MATA(4,9) = -RBG1(2) 
MATB(4) = AXO - MICO 
Y DIRECTION 
MATA(5,5) = 1.0D0 
MATA(5,7) = -RBG1(3) 
MATA(S,9) = RBG1(1) 
MATB(5) = AYO ~ MJCO 
Z DIRECTION 
MATA(6,6) = 1.0D0 
MATA(6,7) = RBG1(2) 
MATA(6,8) = -RBG1(1) 


xe 


460 


ry 


we 


MATB(6) = AZO - MKCO 
SUM OF MOMENTS EQUATIONS 
X DIRECTION 

MATA(7,2) = RBG1(3) 

MATA(7,3) = -RBG1(2) 

MATA(7,7) = -IXXT(1) 

MATA(7,8) = IXYT(1) 

MATA(7,9) = IXZT(1) 

MATA(7,11) = -RAG1(3) 

MATA(7,12) = RAGI1(2) 

MATB(7) = T1X - TOX 
Y DIRECTION 

MATA(8,1) = -RBG1(3) 

MATA(8,3) = RBG1(1) 

MATA(8,7) = IXYT(1) 

MATA(8,8) = -IYYT(1) 

MATA(8,9) = IYZT(1) 

MATA(8,10) = RAG1(3) 

MATA(8,12) = -RAG1(1) 

MATB(8 ) Sy a aenOw. 
Z DIRECTION 

MATA(9,1) = RBG1(2) 

MATA(9,2) = -RBG1(1) 

MABAC9,7) = Tegel) 

MATA(9,8) = IY2T(1) 

MATA(9,9) = -IZ2T(1) 

MATA(9,10) = -RAG1(2) 

MATA(9,11) = RAG1(1) 

MATB(9) = Tig TZ 
LINK TWO 
SUM OF FORCES 
X DIRECTION 

MATA(10,10) = 1.0D0 

MATA(10,13) = M2 

MATA(10,19) = -1.0D0 

MATB( 10) = 0.0D0 
Y DIRECTION 

MATA(11,11) = 1.0D0 

MATA( 11,14) = M2 

MATA(11,20) = -1.0DO 

MATB(11) = 0.0D0 
Z DIRECTION 

MATA(12,12) = 1.0D0 

MATA( 12,15) = M2 

MATA( 12,21) = -1.0D0 

MATB( 12) = -WG2 
EQUATIONS AT JOINT ONE 
X DIRECTION 

MATA(13,4) = -1.0D0 

MATA(13,8) = -RAG1(3) 

MATA(13,9) = RAG1(2) 

MATA(13,13) = 1.0DO0 

MATA( 13,17) = RBG2(3) 

MATA( 13,18) = -RBG2(2) 


106 


als 


we 


phy 


sidety 


ve 


530 


se 


MATB( 13) = MIC1 - MIC2 
Y DIRECTION 
MATA(14,5) = -1.0D0 
MATA(14,7) = RAG1(3) 
MATA(14,9) = -RAG1(1) 
MATA( 14,14) = 1.0D0 
MATA( 14,16) = -RBG2(3) 
MATA(14,18) = RBG2(1) 
MATB( 14) = MJC1 - MJC2 
Z DIRECTION 
MATA(15,6) = -1.0DO 
MATA(15,7) = -RAG1(2) 
MATA(15,8) = RAGI(1) 
MATA(15,15) = 1. 0D0 
MATA(15,16) = RBG2(2) 
MATA(15,17) = -RBG2(1) 
MATB( 15) = MKC1 - MKC2 
SUM OF MOMENTS EQUATIONS 
X DIRECTION 
MATA( 16,11) = RBG2(3) 
MATA( 16,12) = -~RBG2(2) 
MATA(16,16) = -IXXT(2) 
MATA( 16,17) = IXYT(2) 
MATA(16,18) = IXZT(2) 
MATA( 16,20) = -RAG2(3) 
MATA(16,21) = RAG2(2) 
MATB( 16) = T2X - T1X 
Y DIRECTION 
MATA( 17,10) = -RBG2(3) 
MATA( 17,12) = RBG2(1) 
MATA(17,16) = IXYT(2) 
MATA(17,17) = -IYYT(2) 
MATA(17,18) = IYZT(2) 
MATA(17,19) = RAG2(3) 
MATA(17,21) = -RAG2(1) 
MATB( 17) = T2Y - T1Y 
Z DIRECTION 
MATA(18,10) = RBG2(2) 
MATA( 18,11) = -RBG2(1) 
MATA(18,16) = IXZT(2) 
MATA(18,17) = IYZT(2) 
MATA( 18,18) = -IZZT(2) 
MATA( 18,19) = -RAG2(2) 
MATA(18,20) = RAG2(1) 
MATB( 18) = 722 - 112 
LINK THREE 
SUM OF FORCES 
X DIRECTION 
MATA(19,19) = 1.0D0 
MATA( 19,22) = M3 
MATB( 19) = 0.0D0 
Y DIRECTION 
MATA(20,20) = 1.0D0 
MATA( 20,23) = M3 
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X* 


MATB( 20) 

Z DIRECTION 
MATA( 21,21) 
MATA( 21,24) 
MATB(21) = 


0. ODO 


1. ODO 
Me 


“WG 


EQUATIONS AT JOINT TWO 


X DIRECTION 
MATA( 22,13) 
MATA( 22,17) 
MATA( 22,18) 
MATA( 22,22) 
MATA( 22,26) 
MATA( 22,27) 
MATB( 22) 

Y DIRECTION 
MATA( 23,14) 
MATA( 23,16) 
MATA( 23,18) 
MATA( 23,23) 
MATA( 23,25) 
MATA( 23,27) 
MATB( 23) 

Z DIRECTION 
MATA( 24,15) 
MATA( 24,16) 
MATA( 24,17) 
MATA( 24,24) 
MATA( 24,25) 
MATA( 24,26) 
MATB( 24) 


Hou nu ud td un oud ud ou ut 


Hou du we ue a 


Ls OD L8, 
-RAG2(3) 
RAG2( 2) 

1. ODO 
RBG3(3) 
~RBG3( 2) 
Eye = MICS 


-1. 0D0 
RAG2(3) 
-RAG2(1) 

1. ODO 
-RBG3(3) 
RBG3( 1) 
MJC3 - MJC4 


-1. ODO 
-RAG2(2) 
RAG2(1) 

1. ODO 
RBG3(2) 
-RBG3(1) 
MKC3 - MKC4 


SUM OF MOMENTS EQUATIONS 


X DIRBeZ ION 


MATA(25,20) = RBG3(3) 
MATA( 25,21) = -RBG3(2) 
MATA( 25,25) = ~IXXT(3) 
MATAC 25.26) = ipeuic3) 
MATA(25.27) = ezmc3) 
MATB( 25) = ao 

x Y DIRECTION 
MATA( 26,19) = -RBG3(3) 
MATA(26,21) = RBG3(1) 
MATA( 26,25) = IXYT(3) 
MATA(26,26) = -IYYT(3) 
MATA( 26,27) = IYZT(3) 
MATB( 26) = ey 

x Z DIRECTION 
MATA( 27,19) = RBG3(2) 
MATA( 27,20) = -RBG3(1) 
MAAC 27,25) = “iwZm( 3) 
MATA(27,26) = IYZT(3) 
MATA( 27,27) = =1Z29(3) 
MATB( 27) = -na7Z 

weurerre = FIRST LINK IS CONSTRAINED NOT TO ROTATE ‘viewers 
DO 600 I = 4,9 

DO 595 J = 1,27 
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MATA(I,J) = 0. 0DO 
MATA(J,1) = 0.0DO 
595 CONTINUE 
MATB(I) = 0. ODO 
600 CONTINUE 
MATA(4,4) = 1. 0D0 
MATA(5,5) = 1. 0DO 
MATA(6,6) = 1. 0DO 
MATA(7,7) = 1. 0D0 
MATA(8,8) = 1. 0DO 
MATA(9,9) = 1. 0DO 
DOweO2 1 = 13,17 
DO 597 J = 1,27 
MATA(I,J) = 0. ODO 
MATA(J,I) = 0. 0DO 
597 CONTINUE 
MATB(I) = 0. ODO 
602 CONTINUE 
MATA( 13,13) = 1.0D0 
MATA(14,14) = 1. 0D0 
MATA( 15,15) = 1. 0D0 
MATA( 16,16) = 1. 0DO 
MATA( 17,17) = 1. 0D0 
605 DO 610 J = 1,27 
MATA(18,J) = 0. ODO 
MATA(27,J) = 0. 0DO 
610 CONTINUE 
MATA(18,9) = -1.0DO 
MATA(18,18) = 1. 0D0 
MATB( 18) = 0.0D0 
MATA(27,9) = -1.0D0 
MATA(27,27) = 1.0DO0 
MATB( 27) = 0. 0D0 
X CALL EQUATION SOLVER PROGRAM FROM IMSL 
620 CALL LEQT2F(MATA,M,N,IA,MATB, IDGT,WKAREA, IER) 
IF (IER. EQ.0) THEN 
GOTO 640 
ELSE 
WRITE (**,624) IER 
624 FORMAT (15) 


DO 635 I =1, 27 
WRITE (*,627) I 
627 FORMAT (17) 
DO 631 J=1, 27, 3 
WRITE (*,630) J,MATACI,J),J+1,MATACI,J+1) ,J+2,MATA(I,J+2) 


630 FORMAT (15,F13.5,15,F13.5,15,F13.5) 
631 CONTINUE 
WRITE (*,633) I,MATB(1I) 
633 FORMAT (13,F15.5) 
635 CONTINUE 
CALL ENDJOB 
END IF 
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aes'ex 


acres 


“9 


640 


id 


*660 


Sesive 


685 


FIND LCOGX, LCOGY,LCOGZ,THETA VALUES ,WX,WY ,WZ 
MODIFIED BY Rall; VERBGs 


JOINT ZERO 
FXO = MATB(1) 
FYO = MATB(2) 
FZ0 = MATB(3) 
LINK ONE 
SINCE LINK1 1S CONSTRAIN TO ROTATE AROUND THE Z ONLY 
AX1 = 0. OBO 
AY1 = 0.0DO 
AZ1 = 0. 0DO 
AX1 = MATB(4) 
VELX1 = INTGRL(0. ,AX1) 
IONE = INTGRL(X1,VELX1) 
LCOGK(1) = Leeecd 
AY] = MATB(5) 
VELY1 = INTGRL(0. ,AY1) 
Leoer 1 = INTGRL(Y1,VELY1) 
LCOGY(1) = Le@ert 
AZ1 = MATB(6) 
VELZ1 = INTGRL(0. ,AZ1) 
Leoezl = INTGRL(Z1,VELZ1) 
nGOGZ(1) = 16eemi 
WD1X = MATB(7) : 
W1X = INTGRL(O. ,WD1X) 
WDX(1) = WD1X 
W1(1) = W1X 
WD1Y = MATB(8) 
W1Y = INTGRL(0. ,WD1Y) 
WDY(1) = WD1Y 
W1(2) = Wily 
WD1Z = MATB(9) 
W1Z = INTGRL(0. ,WD12Z) 
WD2(1) = WD1Z 
W1(3) =a LZ 
ADDED BY R.M. VERBOS 
THZ = INTGRL(0. ,W1Z) 
THZANG = THZ * RADEG 
COSTHZ = DCOS(THZ) 
SINTHZ = DSIN(THZ) 


IF THE 1ST LINK IS CONSTRAIN TO ROTATE IN THE Z DIRECTION ONLY 
THE DIRECTION COSINE AND DIRECTION COSINE ANGLES ARE CONSTANT 
AND DO NOT NEED TO BE CALCULATED 


CALC DIRECTIONAL COSINES FOR LINK ONE 


LNCOG1 = DSQRT (¢ LCOGX1*LCOGX1 + LCOGY1*LCOGY1 +... 


DRCSX(1) 
DRCSY(1) 
DRCSZ(1) 


L.G0GZ1*LCOGZ1 
WeCGK1 / INGwEel 
LCOGY1 / LNCOG1 
LCOGZ1 / LNCOG1 


AMP = DSQRTCDRCSX(1)*DRCSX(1)+DRCSY(1)*DRCSY(1)+... 


DRCSZ(1)*DRCSZ(1)) 
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wv 
vv 
* 
*720 
oe 


se 


ao’ 


*730 
-\y 
wv 


ry 
735 


ves 


740 


a 


= 


*90 
4 


=e 


DRCSX(1) 
DRCSY(1) 
DRCS2Z( 1) 
DRCANX( 1) 
DRCANY(1) 
DRCANZ(1) 


DRCSX(1)/AMP 
DRCSY(1)/AMP 
DRCSZ(1)/AMP 
DACOS(DRCSX(1)) * RADEG 
DACOS(DRCSY(1)) * RADEG 
DACOS(DRCSZ(1)) * RADEG 


JOINT LOCATION 


ee = Nie * DRCSK(1) 
JY1 = LINKL1 * DRCSY(1) 
JZi = LINKL1 * DRCSZ(1) 
JOINT ONE 

FX1 = MATB(10) 

FY1 = MATB(11) 

FZ1 = MATB(12) 

AX2 = 0. 0DO 

AY2 = 0. 0DO 

AZ2 = 0.0D0 

LINK TWO 

AX2 = MATB(13) 

VELX2 = INTGRL(0. ,AX2) 
ECOG = INTGRI(X2,VELX2) 

me@ex(2) = lneoGK2 

AY2 = MATB(14) 

VELY2 = INTGRL(0. ,AY2) 

LCOGY2 = INTGRL(Y2,VELY2) 

LGOGY(2) = Leocy2 

AZ2 = MATB(15) 

VELZ2 = TNIGEIECO, AZ?) 
LOOGZ2 = INTGRE( 22, VELZ2) 
Eeoesz@2) = LeoGcz2 

WD2X = MATB(16) 

W2X = INTGRL(0. ,WD2X) 

WDX(2) = WD2X 

W2(1) = W2Xx 

WD2Y = MATB(17) 

W2Y = INTGRL(0. ,WD2Y) 

WDY(2) = WD2Y 

W2(2) = W2yY 

WD2Z = MATB(18) 

W2Z = INTGREC 0) Wie) 

WDZ(2) = WD2Z 

W2(3) = W2Z 


GET THE DIRECTION COSINES FOR THE LINK TWO 


DROSX(2) = (LCOGX2 - JX1) / LN@OG2 
DRCSY(2) = (LCOGY2 - JY1) / LNCOG2 
DROSZ(2) = (LCOGZ2 - JZ1) / LNCOG2 


AMP = DSQRT(DRCSX(2)*DRCSX(2)+DRCSY(2)*DRCSY(2)+... 


DRCSZ(2)*DRCSZ(2)) 
DRCSX(2) = DRCSX(2)/AMP 
DRCSY(2) = DRCSY(2)/AMP 
DRCSZ(2) = DRCSZ(2)/AMP 
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* DRCANX( 2 ) 


DACOS(DRCSX(2)) * RADEG 


¥ DRCANY(2) = DACOS(DRCSY(2)) %* RADEG 
ve DRCANZ(2) = DACOS(DRCSZ(2)) * RADEG 
* JOINT LOCATION 
800 JX2 = JX1 + LINKL2 * DRCSX(2) 
JY¥2 = JY1 + LINKL2 * DRCSY(2) 
JZ2 = JZ1 + LINKL2 * DRCSZ(2) 
5 JOINT TWO 
805 FX2 = MATB(19) 
FY2 = MATB(20) 
FZ2 = MATB(21) 
were LINK THREE 
812 AX3 = MATB( 22) 
VELX3 = INTGRL(0. ,AX3) 
LCOGX3. = INTGRL(X3,VELX3) 
LCOGX(3) = 1e0ex3 
AY3 = MATB( 23) 
VELY3 = INTGRL(V3IC,AY3) 
LCOGY3 = INTGRL(Y3,VELY3) 
LCOGY(3) = LCOGY3 
AZ3 = MATB( 24) 
VELZ3 = INTGRL(0. ,AZ3) 
LCOGZ3 = INTGRL(23, VEma3) 
LCOGZ(3) = LCOGZ3 
WD3X = MATB(25) 
W3X = INTGRL(W3IC,WD3X) 
WDX(3) = WD3X 
W3(1) = W3X 
WD3Y = MATB( 26) 
W3Y = INTGRL(0. ,WD3Y) 
WDY(3) = WD3Y 
W3(2) = W3Y 
WD3Z = MATB( 27) 
W3Z = INTGRL(0. ,WD3Z) 
WDZ(3) = WD3Z 
W3(3) = W3Z 


* CALC DIRECTIONAL 
845 DRCSX(3) 
DRCSY(3) 
DRCSZ( 3) 


865 


DRCSX(3) 
DRCSY(3) 
DRCSZ(3) 
DRCANX(3) 
DRCANY( 3) 
DRCANZ( 3) 


* TIP LOCATION 
875 


COSINES FOR LINK THREE 
(LCOGX3 - JX2) / LNCOG3 
(LCOGY3 - JY2) / LNCOG3 
(LCOGZ3 - JZ2) / LNCOG3 
AMP = DSQRT(DRCSX(3)*DRCSX(3)+DRCSY(3)*DRCSY(3)+... 


DRCSZ(3)*DRCSZ( 3) ) 


DRCSX(3) /AMP 
DRCSY(3) /AMP 
DRCS2(3)/AMP 


DACOS(CDRCSX(3)) * RADEG 
DACOS(DRCSY(3)) * RADEG 
DACOS(DRCSZ(3)) * RADEG 


TIPX = JX2 + LINKL3 * DRCSX(3) 


TIPY = JY2 + ENKI “oDRESY( 3) 


2 


TIPZ = JZ22 + LINKL3 * DRCSZ(3) 


* FIND THE 
80 ANG23X 


ANG23Y = 
ANG23Z = 
ANG12X = 
ANG12Y = 
ANG12Z = 
DYNAMIC 
NOSORT 


ANGLE BETWEEN THE LIMKS 
DRCANX(2) - DRCANX(3) 
DRCANY(2) - DRCANY(3) 
DRCANZ(2) - DRCANZ(3) 
DRCANX( 1) - DRCANX( 2) 
DRCANY( 1) - DRCANY(2) 
DRCANZ( 1) - DRCANZ(2) 


pe §6INPUT TORQUE AT JOINTS 
PanNeee 1375) * SIN (Pl / (30,0) * TIME)) * RONFAC 


T2 = T2FNC 
T2X = T2 
END 
STOP 
FORTRAN 
* SUBROUTINE TO COMPUTE THE CROSS PRODUCT OF TWO VECTORS 


SUBROUTINE CPROD(VECTA, VECTB ,MI ,MJ,MK) 
940 IMPLICIT REAL*8 (A-Z) 
DPUMENSION VECTA(3).VECTB( 3) 


MI = 
MJ = 
MK = 
RETURN 

END 


VECTAC2) * VECTBC3) - VECTAC3) * VECTB( 2) 
POAC s je VECTRG@H) - VECTAC(1) * VECTB(3) 
VECTAC(1) * VECTB(2) - VECTAC2) * VECTB(1) 
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APPENDIX E 


BASIC OPERATING INSTRUCTIONS FOR THE NAVAL 
POSTGRADUATE SCHOOL RIGID MANIPULATOR TEST BED 


A. SYSTEM FAMILIARIZATION 

Before operating the manipulator test bed, you should ensure you 
are familiar with its components and basic procedures of operation. If 
you are already familiar with the components of the manipulator and 
their set-up, you will need to review this section to operate the 
hardware. 

The test bed consists of four major sub-systems: the Neptune II 
hydraulically operated manipulator; an IBM PC-AT used to control the 
manipulator and gather data (at the present time); an IBM PC-XT (not 
in the system at present); and an Electronic Control Panel used in 
switching, data gathering, and interfacing. 

1. The Manipulator 

The manipulator (Figure E-1), being a hydraulic mechanism, 
is subject to leakage. It is advisable to wear old clothing while in the 
lab. The operating pressure of the system is around 100 psi and a 
minor leak may spray anywhere in the lab. 

The source of the hydraulic fluid is the pump cabinet against 
the forward side of the wall in front of the computers. This cabinet 
contains a pump which is operated from the ECP, a reservoir which is 
underneath the pump, and an accumulator that is alongside of the 


pump (Figure E-2). 





Figure E-1. Manipulator 
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Figure E-2. Pump Cabinet 
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The reservoir has a fill hole on the top. It is necessary to 
check this before operating the system and ensure that the level is 
around half when the system is not in operation. You will also notice a 
pressure gage which reads the pressure in the accumulator. This 
pressure should be between 80 and 100 psi and may be increased by 
pressurizing the bladder in the accumulator to a higher pressure. 

If you look closely at the accumulator, you will notice a 
solenoid valve at the outlet. This valve has been installed because of 
previous problems and will cut off all hydraulic flow to the manipulator 
when it is shut from the ECP. 

Underneath the manipulator itself is a set of Electronic 
Printed Circuit Boards (EPCB). These boards control the opening and 
shutting of the solenoid values to the joint pistons (see Ref. 9 for more 
information). The important thing to know about these boards is that 
you should avoid getting hydraulic fluid on them. 

2. The Computers 

At present, the IBM-AT is the only computer hooked up in 
the system. It is easy to follow because it has been set up with a menu- 
based operating system. The power to the AT is under the right side of 
the computer stand. The first thing that will happen when you turn 
the power on is that the AT will go thru a cold-start boot-up, which 
may take a minute or two. You will then notice a listing of subdirecto- 
ries on the screen. Most of these subdirectories are of little use 
because the files are proprietary in nature and the author has long 


since transferred away from NPS, but feel free to skim through them. 


For the initial running of the manipulator, you will want to 
select the BATCHES subdirectory. Once you are in this subdirectory, 
you will select the NEPTROLA file to run the manipulator. This file 
contains the control program for operating the manipulator in the 
solenoid mode. The program is a useful starting point for examining 
the range of the arm motion. 

NEPTROLA is a menu-driven set-up that asks various ques- 
tions to get the system in the proper operating configuration. The 
actual operation will be discussed in the operation section of this 
appendix. 

3. Electronic Control Panel (ECP) 

The ECP is located between the IBM-AT and the IBM-XT and 
is the heart of the control of the manipulator. The ECP contains sev- 
eral interface boards which should not be tampered with without 
talking to Tom Cristian of the Mechanical Engineering technical staff. 
The front of the panel is divided into four sections (Figure E-3). Start- 
ing at the top, the first sub-panel consists of two sets of electrical 
jacks. The first six from the left are connected to the position data of 
the interface boards and will provide joint O thru 5 position output. 
The last two jacks are for servo operation which is no longer con- 
nected (more information on servo operation is available in Ref. 13). 
The next panel down was for control of the servovalves; again, more 
information is available in Reference 13. The next panel contains a 


power switch which selects power to the panel and electrical jacks to 
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Figure E-3. Electronic Control Panel (ECP) 


119 


monitor the pressure transducer output for torque calculations. Pres- 


sure transducers have been installed on joints 1 and 2. The electrical 


jacks are wired so that Pl and P2 are connected to joint 1 transduc- 


ers, P3 and P4 are connected to joint 2 transducers, and P5 and P6 


are available for further connections. 


The bottom panel is the power panel and consists of seven 


switches and three electrical jacks. The switches, from left to right, 


are as follows: 


Electronic Control Power, which supplies the electricity to the 
interface board at the back of the panel. 


Solenoid Current Switch, which must be “ON” to operate the 
manipulator in the solenoid mode. 

Servo Master Switches (3), which are three-position switches: 

- (DOWN) is for selecting servovalve control from the AT 

- (MIDDLE) is for operating in the solenoid mode 


- (UP) is for for operating the servovalves from an auxiliary signal 
which is connected to the three electrical jacks above the 
switches. 


Extra switch. 

(Rightmost) Pump Power/Solenoid Valve Control Switch. This is a 
three-position switch: 

— (UP) the pump is in the run mode and controlled by pressure 

- (MIDDLE) the pump is off and the solenoid valve is open 


- (DOWN) the pump is off and the solenoid valve has power to it 
and is shut (this is an emergency cut-off position). 
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B. OPERATING THE ARM 
1. The Solenoid Mode 


Following is a step-by-step procedure for operating the Nep- 
tune II manipulator in the solenoid mode: 


ora | 


Check the accumulator to ensure that there is sufficient fluid to 
operate the manipulator. If the level is low, add a 50/50 mix of 
Hydro Lube and water until the level is between one-half and 
three-quarters (DO NOT FILL). 

STEP 2 


Check the accumulator pressure to ensure it is between 80 and 
100 psi. If the pressure is low or high, bleed or add air through 
the hose connection as necessary. 


SanP 3 


From the ECP turn the following switches to the position referred 
to: 


- Electronic Power Switch ON (UP) 
- Solenoid Current Switch ON (UP) 


— Servo Master Switches OFF (MIDDLE) 
- Pump Power Switch ON (UP) 
STEP 4 


Now turn the IBM-AT computer on. 


SEP’ 5 
Select the BATCHES subdirectory. 


STEP 6 


Select the NEPTROLA program and answer “yes” (Y) to the ques- 
tion whether you want to operate without ADC. This will allow you 
to operate from the keyboard. The next question will give several 
options— pick the keyboard option. Follow the given computer 
menu and vary the joint positions as desired. 
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STEP 7 
To leave the NEPTROLA program, hit ESC, then hit the control 
and break key at the same time. After this, type “SYSTEM” and 
you will be returned to the BATCHES subdirectory menu. 
2. Servovalve Operation 
At present, the servo valves have been removed from the sys- 


tem but the software and wiring are still available. When the servo- 


valves are reinstalled, refer to [Ref. 13] for operating procedures. 
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